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(57) A floor shape estimation system for a legged 
mobile robot in particular a biped walking robot, which 
estimates a floor shape (130) J more specifically a foot- 
to-foot floor inclination difference based on at least a 
control error of the total floor reaction force's moment 
based on the estimated value and corrects a feet com- 
pensating angle (114b) based on the estimated value. 
Further, the system estimates a floor shape (130), more 
specifically a foot floor inclination difference based on 



at least a control error of the foot floor reaction force 
about a desired foot floor reaction force central point and 
corrects afoot compensating angle (114c) based on the 
estimated value. With this, it can accurately estimate the 
shape of a floor with which the robot is in contact. Fur- 
ther, if the floor shape is different from what has been 
supposed, it can produce the floor reaction force as de- 
sired by absorbing the influence of difference. 
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Description 

TECHNICAL FIELD TO WHICH THE INVENTION RELATES 
[0001] The present invention relate ^ 

o a system for estimating the shape such as inclination of the f ioor with which a legged mob e robot ,s m contact and 
in addition, to a system for enabling to control a floor reaction force based on the estimated floor shape. 

BACKGROUND ART OF THE INVENTION 

[0002] As floor shape estimation prior art, there are known a technique described in Japanese LaW-Opf," Patent 
Application No Hei 6(1 994)-31 658 proposed by the applicant, and another technique mentioned on a paper entitled 
SSpment of a Biped Walking Robot Adapting to an Unknown Uneven Surface" (Journal of the Robotics Sooty 

^^^'^'^,!!^ on the journal) wii, be summarized in the fo.lowing and problems thereof wiii 

iSo^T Tthfs'rior art, the robot foot is configured to have three stories in height comprising an upper-fool r plate a 
cushioning section and a lower-foot plate (sole). The foot has potentiometers at four comers which detect the angle 
and the dttance between the upper and lower-foot plates, and a clinometer (inclination sensor) which detects the 
absolute inclination of the upper-foot plate. The foot is also provided with spikes at the four corners. 
[OOoT is Prior art, when a free leg is landed, the foot is controlied to profile the floor surface ba.ec I on the 
nformation obtained from the potentiometers and the clinometer. With this the spikes O^^Jf^ 1 ^^ 
are in contact with the floor such that the lower-foot plate is in parallel with the floor surface. At that insta nt J he , he ght 
and the angle of inclination of the floor are estimated through geometric calculation based on the ff^f ^ 
position and/or posture of the upper-foot plate obtained by the clinometer and the angle of the upper-foot plate relative 
to t e lLerlot P p,ate and the distance between the upper and lower-foot plates obtained based on the poten torn. ers. 
[0006] However, this prior art technique has the following problems. Specifically, it can not conduc a posture stabi- 
ization control to manipulate the floor reaction force until the foot has landed on the floor to profile the floor surface 
Further, if a member made of a soft material such as rubber is applied to the foot sole so as to enhance grip, the 
structure would disadvantageously increase estimation error. 

[0007] Further, when the foot does not land on the floor completely, i.e., when the foot does not land wrth its sole 
surface-contact with the floor, it can not estimate a floor height correctly during the two-leg supporting period. Further, 
when thefeetareincontact with the floor completely, for example, at the time of holding itself u f'9M, it can not estimate 
* the same time, the inclination of a surface with which each foot is in contact and a height d.fference between the 
surfaces with which the feet are in contact. Furthermore, since it can only estimate the floor shape at an instant of foot 
landinq if the floor deforms after landing, it can no longer estimate the deformed shape. 

[0008] On the other hand, the former (i.e., that described in 6 (1994)-31658)) can estimate the inclination of floor, 
while conducting a posture stabilization control to manipulate the floor reaction force. However, during the two- eg 
supporting period it can only estimate the inclination of a surface that intersects or adjoins the surfaces with which , the 
feet are in contact, whilst during one-leg supporting period it can only estimate the inclination of the surface with which 
the supporting leg foot is in contact. In other words, this prior art technique can not estimate the inclinations at the 
same time. 

DISCLOSURE OF THE INVENTION 

[0009] An object of the present invention is to solve the problem and to provide a floor shape estimation system of 
a legged mobile robot which can accurately conduct simultaneous floor shape estimation which has been difficult in 
the prior art i e which can accurately estimate a floor shape, more specifically the inclination of the surface with which 
the foot is in contact and a height difference between the surfaces with which the feet are in contact, at the same time, 
in other words in a composite manner. ■ *nuaaa\ 

[0010] Aside from the above, the applicant proposes, in Japanese Laid-Open Patent Application No. Hei 1 0(1 998)- 
277969 detecting the posture inclination of a legged mobil robot and determining a compensating total floor reaction 
force's moment about a desired total floor reaction force central point based on the detected posture and inclination, 
distributing it to each foot such that the position and/or posture of each feet rotates about the desired total foot floor 
reaction force central point and desired each foot floor reaction force central point by predetermined amounts respec- 
tively such that the robot walks with stable posture even on a floor that has not only a widely-spreading slant or undu- 
lations, but also has local slants or undulations. In „„ ari 
[0011] A second object of the present invention is to provide a floor shape estimation system of a legged mobile 
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robot which can correct a foot trajectory or pattern by the estimated floor shape, whereby enabling to bring a steady- 
state error of the actual floor reaction force from a desired value in the control, which has not been eliminated in the 
prior art, close to zero as much as possible, in other words to solve the steady-state error due to a deviation of the 
floor reaction force. 

5 [0012] In order to achieve the above-mentioned objects, there is provided as defined in claim 1 a system for controlling 
a legged mobile robot having at least a body and a plurality of legs each connected to the body through a first joint 
and having a foot connected to its distal end through a second joint, the system having a compliance mechanism which 
deforms in response to a floor reaction force acting from a floor with which the foot is in contact and joint displacement 
means which displacing the first and second joints such that the foot follows a desired position and/or posture, com- 

10 prising: inclination detection means for detecting an inclination of the body relative to a vertical direction; walking en- 
vironment foot position and/or posture calculating means forcalculating a position and/or posture of the foot in a walking 
environment including the floor with which the foot is in contact, based on an actual and/or desired position and/or 
posture of the foot and the detected inclination; deformed foot position/posture calculating means for calculating the 
position and/or posture of the foot with deformation of the mechanism in response to the floor reaction force acting 

15 from the floor in the walking environment using a mechanism compliance model which models the compliance mech- 
anism; and floor shape estimation value calculating means for calculating a floor shape estimation value indicative of 
shape of surface of the floor with which each foot is in contact and indicative of a relationships therebetween. With 
this, the system can accurately estimate the inclination of the surface with which the foot is in contact and a height 
difference between the surfaces with which the feet are in contact, at the same time, in other words in a composite 

20 manner, that has been difficult in the prior art. 

[0013] In the system, the compliance model comprises a first model which expresses the floor reaction force acting 
on each foot and a change of position and/or posture of each foot resulting from the floor reaction force acting on each 
foot, and a second model which expresses the floor reaction force acting at a desired total floor reaction force central 
point and a change of one of the foot relative to the other resulting from the floor reaction force acting at the desired 

25 total floor reaction force central point. With this, the system can accurately estimate the inclination of the surface with 
which the foot is in contact and a height difference between the surfaces with which the feet are in contact with a simpler 
configuration, at the same time : in other words in a composite manner. 

[0014] In the system, the floor shape estimation value calculating means has a filter which removes a high-frequency 
component. With this, the system can accurately estimate the inclination of the surface with which the foot is in contact 
30 and a height difference between the surfaces with which the feet are in contact, at the same time, in other words in a 
composite manner. And the system can prevent the control system from oscillating. 

[0015] The system further includes: correcting means for correcting the desired position and/or posture of the foot 
based on the calculated floor shape estimation value. With this, in addition to the advantages and effects mentioned 
above, the system can bring a steady-state error of the actual floor reaction force from a desired value in the control, 
35 which has not been eliminated in the prior art, close to zero as much as possible, in other words to solve the steady- 
state error due to a deviation of the floor reaction force. 

[0016] The system further includes; gait generating means for generating a gait of the robot including at least a 
motion pattern of the robot including at least the desired position and/or posture of the foot and a desired pattern of a 
total floor reaction force acting on the robot; desired foot floor reaction force central point determining means for de- 

JO termining a desired foot floor reaction force which is a central point of action on the foot when the total floor reaction 
force of the generated gait is distributed to each of the foot; actual floor reaction force detecting means for detecting 
an actual floor reaction force acting on the foot; and foot rotation amount determining means for calculating a moment 
of the detected actual floor reaction force acting about the calculated foot floor reaction force central point, and for 
determining a foot rotation amount for rotating the foot based on the calculated moment wherein the correcting means 

^5 includes: foot rotation amount correcting means for correcting the foot rotation amount based on the calculated floor 
shape estimation value; and foot position/posture correcting means for correcting the desired position and/or posture 
of the foot based on the corrected foot rotation amount such that the position/posture of the foot rotates. With this, in 
addition to the advantages and effects mentioned above, the system can control the floor reaction force acting on the 
floor, without causing a conflict or interference, easily and appropriately. 

50 [0017] The system further includes: gait generating means for generating gait of the robot including at least a motion 
pattern of the robot including at least the desired position and/or posture of the foot and a desired pattern of a total 
floor reaction force acting on the robot; desired foot floor reaction force central point determining means for determining 
a desired foot floor reaction force which is a central point of action on the foot when the total floor reaction force of the 
generated gait is distributed to each of the foot; actual floor reaction force detecting means for detecting an actual floor 

55 reaction force acting on the foot; and foot rotation amount determining means for determining a foot rotation amount 
for rotating the foot based on the detected actual floor reaction force; wherein the correcting means includes: foot 
rotation amount correcting means for correcting the foot rotation amount based on the calculated floor shape estimation 
value; and foot position/posture correcting means for correcting the desired position and/or posture of the foot based 
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on the corrected foot rotation amounts such that the position and/or posture of the foot rotates about the determined 
foot floor reaction force central point or thereabout. With this, in addition to the advantages and effects mentioned 
above, the system can control the floor reaction force acting on the floor, without causing a conflict or interference, 
easily and appropriately. 

5 [0018] In the system, the foot position/posture correcting means corrects the desired position and/or posture of the 
foot based on the corrected foot rotation amounts such that the position and/or posture of the foot rotates about the 
determined foot floor reaction force central point or thereabout. With this, in addition to the advantages and effects 
mentioned above, the system can control the floor reaction force acting on the floor, without causing a conflict or 
interference, easily and appropriately. 

io [0019] The system further includes: foot movement amount determining means for calculating at least one of the 
total floor reaction force's moment actually acting on the robot, and a moment obtained by subtracting the moment of 
the detected actual floor reaction force acting about the calculated foot floor reaction force central point, from the total 
floor reaction force's moment actually acting on the robot, and for determining a foot movement amount for moving the 
foot based on the calculated moment. With this, in addition to the advantages and effects mentioned above, the system 

15 can control the total floor reaction force that is significant in particular in a posture stabilization control, more appropri- 
ately. 

[0020] In the system, the foot position/posture correcting means further corrects the desired position and/or posture 
of the foot based on an posture error of the robot. With this, in addition to the effects and advantages mentioned above, 
the system can control the robot posture more stably. 

20 [0021] In the system, a compensating total floor reaction force's moment for posture stabilization that is to be added 
to the desired pattern of the total floor reaction force is determined, and the foot rotation amount determining means 
and/or the foot movement amount determining means determines the foot rotation amount and/or the foot movement 
amount based on at least the detected actual floor reaction force and the determined compensating total floor reaction 
force's moment for posture stabilization. With this, in addition to the advantages and effects mentioned above, the 

25 system can control the floor reaction force more appropriately for the robot posture stabilization. 

[0022] In the system, the compensating total floor reaction force's moment for posture stabilization is determined 
based on an inclination error of the robot. With this, in addition to the advantages and effects mentioned above, the 
system can further improve the robot posture stabilization. 

[0023] In the system, the foot rotation amount determining means and/or the foot movement amount determining 

30 means determines the foot rotation amount and/or the foot movement amount such that the determined compensating 
total floor reaction force's moment for posture stabilization is distributed to each of the legs. With this, in addition to 
the advantages and effects mentioned above, the system can distribute the load of robot to the plurality of legs appro- 
priately and can prevent the distribution of pressure with the floor from being excessive in a local portion. 
[0024] The system further includes: gait generating means for generating gait of the robot including at least a motion 

35 pattern of the robot including at least the desired position and/or posture of the foot and a desired trajectory pattern of 
a total floor reaction force acting on the robot; compensating total floor reaction force calculating means for calculating 
a compensating total floor reaction force for posture stabilization; actual floor reaction force detecting means for de- 
tecting an actual floor reaction force acting on the foot; and floor reaction force distributing means for distributing the 
total floor reaction force of the generated gait and the compensating total floor reaction force; wherein the correcting 

40 means corrects the desired foot position and/or posture based on the distributed total floor reaction force of the gen- 
erated gait and the compensating total floor reaction force, the detected actual floor reaction force and the calculated 
floor shape estimation value. With this, in addition to the advantages and effects mentioned above, the system can 
distribute an amount of foot correction more appropriately, thereby enabling to achieve greater force of restoration and 
the ability of contact with the floor for the robot posture stabilization. 

45 [0025] In the system, the compensating total floor reaction force for posture stabilization is calculated based on an 
inclination error of the robot. With this, in addition to the advantages and effects mentioned above, the system can 
further improve the robot posture stabilization. 

BRIEF DESCRIPTION OF THE DRAWINGS 

50 

[0026] 

Figure 1 is a schematic view showing an overall configuration of a legged mobile robot, more specifically a biped 
walking robot to which a floor shape estimation system of a legged mobile robot according to an embodiment of 
55 the present invention is applied; 

Figure 2 is an explanatory side view showing the structure of the foot of a biped robot illustrated in Figure 1 ; 

Figure 3 is a block diagram showing details of a control unit of the robot illustrated in Figure 1 ; 

Figure 4 is a block diagram showing the configuration and operation of the control system of a legged mobile robot 



4 

BNSDOCIJ; <EP 1291 136A1_!_.> 



EP1 291 136 A1 



proposed earlier and the floor shape estimation system of a legged mobile robot according to the embodiment of 
present invention, in a functional manner; 

Figure 5 are explanatory views showing one example of the motion pattern of the robot illustrated in Figure 1 when 
it walks on a flat floor; 

Figure 6 is an explanatory plan view showing the trajectory on floor surface of a desired total floor reaction force 
central point (desired ZMP) corresponding to the motion pattern illustrated in Figure 5; 

Figure 7 is a time chart showing the trajectory of the desired total floor reaction force central point (desired ZMP) 
corresponding to the motion pattern illustrated in Figure 5; 

Figure 8 is a time chart showing the trajectory of a desired 1st foot floor reaction force central point, which is set 

to satisfy predetermined conditions, corresponding to the motion pattern illustrated in Figure 5; 

Figure 9 is a time chart showing the trajectory of the desired 2nd foot floor reaction force central point, which is 

set to satisfy the predetermined conditions, corresponding to the motion pattern illustrated in Figure 5; 

Figure 1 0 is a flow chart showing, similarly to Figure 4, the operation of a control system of a legged mobile robot 

according to the present invention; 

Figure 11 is an explanatory view showing a situation that actual foot floor reaction force acts on each of the 1st 
foot and the 2nd foot during the two-leg supporting period, for explaining the operation of a composite compliance 
operation determinator illustrated in Figure 4 which conducts calculations of a feet compensating angle, etc., re- 
ferred to in the flow chart of Figure 10; 

Figure 1 2 is an explanatory view showing setting of the desired total floor reaction force in the situation illustrated 
in Figure 11; 

Figure 13 is an explanatory view showing distribution of the desired foot floor reaction force in the situation illus- 
trated in Figure 11 ; 

Figure 1 4 is an explanatory view showing a compensating total floor reaction force's moment in the situation illus- 
trated in Figure 11; 

Figure 15 is an explanatory view showing a vector V which is normal on the plane that includes the foot floor 
reaction force centra! point and is perpendicular to the horizontal plane in the situation illustrated in Figure 11 ; 
Figure 1 6 is an explanatory view showing the desired foot floor reaction force central point rotated about the desired 
total floor reaction force central point (desired ZMP) by a predetermined angle 6dbv in the situation illustrated in 
Figure 1 1 ; 

Figure 1 7 is an explanatory view showing rotation of the feet about the back-and-forth-axis and I eft-and- right-ax is 
by predetermined angles 9nx, Gny in the situation illustrated in Figure 16; 

Figure 1 8 is a block diagram showing the calculation of a composite compliance operation determinator illustrated 
in Figure 4; 

Figure 1 9 is a block diagram showing the calculation of a compensating total floor reaction force's moment dis- 
tributor illustrated in Figure 18; 

Figure 20 is a set of time charts each showing examples of set distribution-weight variables of the compensating 
total floor reaction force's moment distributor, illustrated in Figure 18, for manipulating a feet compensating angle; 
Figure 21 is an explanatory view showing a robot posture for explaining the setting of the distribution-weight var- 
iables of the compensating total floor reaction force's moment distributor illustrated in Figure 20; 
Figure 22 is an explanatory view, similar to Figure 21 , also showing a robot posture for explaining setting of the 
distribution-weight variables of the compensating total floor reaction force's moment distributor; 
Figure 23 is an explanatory view showing a V-direction-component of the feet compensating moment Mdmddbv 
when the distributor-weight variables for manipulating the feet compensating angle are determined under a pre- 
determined condition; 

Figure 24 is a block diagram showing the calculation of a feet compensating angle determinator illustrated in Figure 
18; 

Figure 25 is a block diagram showing the calculation of a foot compensating angle determinators illustrated in 
Figure 18; 

Figure 26 is an explanatory view showing the calculation of a corrected desired foot position and/or posture with 
deformation compensation calculator illustrated in Figure 18; 

Figure 27 is an explanatory view, similar to Figure 26, showing the calculation of a corrected desired feet position 
and/or posture with deformation compensation calculator illustrated in Figure 18; 

Figure 28 is a flow chart showing the subroutine for determining the feet compensating angle, etc., referred to in 
the flow chart of Figure 1 0; 

Figure 29 is an explanatory view, similar to Figure 16, but showing a modification of a control system proposed 
earlier and illustrating another example of foot position correction; 

Figure 30 is a block diagram, similar to Figure 18, but showing the configuration of the floor shape estimation 
system of the legged mobile robot according to the embodiment of the present invention; 



5 



...1291 136A1 I. > 



EP 1 291 136 A1 



Figure 31 is an explanatory view showing definition of terms which the system illustrated in Figure 30 uses in floor 
shape estimation; 

Figure 32 is an explanatory view also showing definition of terms which the system illustrated in Figure 30 uses 
in floor shape estimation; 

5 Figure 33 is a block diagram showing the processing of a floor shape estimator in the system illustrated in Figure 

30 in a functional manner; 

Figure 34 is a block diagram showing the processing of a mechanism compliance model in thefloor shape estimator 
illustrated in Figure 33 in afunctional manner; 

Figure 35 is a block diagram also showing the processing of the mechanism compliance model in the floor shape 
io estimator illustrated in Figure 33 in a functional manner; 

Figure 36 is an explanatory view showing oscillation of foot floor inclination difference estimated value obtained 
in the embodiment of the present invention; 

Figure 37 is a flow ch art showing the processing of a foot floor inclination difference estimation interrupter according 
to the embodiment of the present invention; 
75 Figure 38 is a time chart showing the processing of Figure 37; 

Figure 39 is an explanatory view explaining the processing of Figure 37; 

Figure 40 is a flow chart showing the processing of a foot-to-foot floor inclination difference estimation interrupter 
according to the embodiment of the present invention; 
Figure 41 is a time chart showing the processing of Figure 40; 
20 Figure 42 is a block diagram, similar to Figure 30, but showing the configuration of a floor shape estimation system 

of a legged mobile robot according to a second embodiment of the present invention; 

Figure 43 is an explanatory view showing definition of terms which are used in the system according to the second 
embodiment illustrated in Figure 42; 

Figure 44 is a block diagram showing the processing of a foot-to-foot floor inclination difference estimator of the 
25 system according to the second embodiment; 

Figure 45 is a block diagram showing the processing of a foot floor inclination difference estimator of the system 
according to the second embodiment; 

Figure 46 is an explanatory view showing another example of a sensor to be used in the floor shape estimation 
system according to the first and second embodiments; 
so and 

Figure 47 is an explanatory view showing still another example of a sensor to be used in the floor shape estimation 
system according to the first and second embodiments. 

3 EST MODES FOR CARRYING OUT THE INVENTION 

35 

[0027] The floor shape estimation system of a legged mobile robot according to an embodiment of the present in- 
vention will be explained with reference to the accompanied drawings. A biped robot is taken as an example of the 
legged mobile robot. 

[0028] Figure 1 is a schematic view showing an overall configuration of the legged mobile robot, more specifically a 
40 biped robot to which the floor shape estimation system according to the present invention is applied. 

[0029] As illustrated in the figure, a biped robot (hereinafter simply referred to "robot") 1 has a pair of right and left 
legs (leg links) 2 each composed of six joints. (In a simple representation, each of the joints is represented by an electric 
motor which actuates the joint.) The six joints include, arranged successively downward, a pair of hip joints 1 OR, 10L 
(the right-hand joint is indicated by R and the left-hand joint by L) for rotating legs with respect to hips, a pair of hip 
45 joints 12R, 12L in the rolling axis (about an X-axis), a pair of hip joints 14R, 14L in the pitching axis (about a Y-axis), 
a pair of knee joints 16R, 16L in the pitching axis, a pair of ankle joints 18R, 18L in the pitching axis, and a pair of joints 
20R 20L in the rolling axis. 

[0030] The robot is provided with feet 22R, 22L underneath of the joints 1 8R(L) and 20R(L), and a body (trunk) 24 
at its top which houses a control unit 26 comprising microcomputers (explained later). In the above, the joints 10R(L), 
50 1 2R(L), 1 4R(L) make up the hip joints, the joint 1 6R(L) makes up the knee joint, and the joints 1 8R(L), 20R(L) make 
up the ankle joints. The hip joints and knee joints are connected to each other by thigh links 28R, 28L, and the knee 
joints and ankle joints are connected to each other by cms or shank links 30R, 30L. 

[0031] With the above structure, each of the legs 2 is given six degrees of freedom. When the 6*2= 12 joints are 
driven to suitable angles during walking, a desired motion is imparted to the entire leg structure to cause the robot to 
55 walk arbitrarily in a walking environment of three-dimensional (absolute) space. (In the specification, "*" represents 
multiplication in scalar calculation and outer product in vector calculation.). 

[0032] It should be noted that the position and velocity of the body 24 indicate the position and its displacement 
velocity of a predetermined position of the body 24, specifically a representative position such as the position of the 
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center of gravity of the body 24. 

[0033] As shown in Figure 1 , a known force sensor (more precisely, known as the six-axis force and torque sensor) 
34 is disposed at a position below each ankle joint for generating a signal indicative of three directional components 
Fx, Fy, Fz of force and three directional components Mx, My, Mz of torque or moment thereby of the force and outputs 

5 a signal indicative of foot landing and the floor reaction force (the load acting from the floor). Moreover, the body 24 
nas an inclination sensor 36 which generates a signal indicative of tipping or inclination in the frontal plane with respect 
to a Z-axis (the vertical direction (the direction of gravity)) and its angular velocity (yaw-rate), and also a tilt in the 
sagittal plane with respect to the Z-axis and its angular velocity. The electric motors of the respective joints are coupled 
with respective rotary encoders which generate signals indicative of angular displacements of the electric motors. 

10 [0034] As illustrated in Figure 2, a spring mechanism 38 is installed at a position upward from the foot 22R(L), which 
constitutes a compliance mechanism 42 together with a sole elastic member 40 attached to the sole of the foot 22R 
(L). The spring mechanism 38 has a box-like guide member (not shown) connected to the foot 22R(L), and a piston 
member (not shown) connected to the side of the ankle joint 1 8R(L), 20R(L) and the force sensor 34 and inserted in 
the guide member with an elastic member such that it moves in the guide member to a slight extent. 

15 [0035] In the figure, the foot 22R(L) illustrated in thick lines shows the foot which is not subject to the floor reaction 
f orce. When subjected to the floor reaction force, the spring mechanism 32 and the sole elastic member 34, which 
constitute the compliance mechanism 42, deform such that the foot 22R(L) shifts to the position and/or posture illus- 
trated in dashed lines. This configuration is significant not only for decreasing the foot-landing impact, but also for 
enhancing the control performance. Since this configuration is disclosed in the aforesaid application (Japanese Laid- 

20 Open Patent Application No. Hei 5 (1993) - 305, 584), no further explanation will be made. 

[0036] Although not shown in Figure 1 , a joystick 44 is provided at an appropriate location of the biped robot 1 , which 
enables an operator from the outside to input a request concerning the gait, such as switching from moving forward 
to turning. 

[0037] Figure 3 is a block diagram which shows the details of the control unit 26, which is comprised of microcom- 
25 outers. Outputs from the inclination sensor 36, etc., are converted by an A/D converter 50 into digital signals, which 
are transmitted through a bus 52 to a RAM 54. Output signals from the rotary encoders that are disposed adjacent to 
the respective electric motors are transmitted through a counter 56 to the RAM 54. 

[0038] The control unit 26 includes a first calculator 60 and a second calculator 62 respective of which comprises a 
CPU. The first calculator 80 estimates the shape of a floor (floor surface) with which the robot 1 is in contact as will be 
so explained later, calculates joint angle displacement commands as will be explained later, and outputs the same to the 
RAM 54. The second calculator 62 reads the desired command values and the detected joint angles from the RAM 
74, and calculates values (manipulated variables) and outputs the same through a D/A converter 86 and servo-ampli- 
fiers to the electric motors which drive the respective joints. 

[0039] Briefly explaining the floor shape estimation system of a legged mobile robot according to this embodiment, 
35 ;t is an improvement of the technique described in Japanese Laid-Open Patent Application No. Hei 6(1994)-31 658 
proposed by the applicant. Specifically, the system is a development obtained by expanding the technique described 
n Japanese Laid-Open Patent Application Hei 6(1 994)-31 658, based on the technique described in Japanese Laid- 
Open Patent Application No. Hei 10(199B)-277969 proposed by the applicant earlier. 

[0040] More specifically, in the technique proposed by 6 (1994)-31658, based on a control error of the total floor 
40 taction force's moment about a ZMP, a foot-to-foot inclination difference is estimated during the two-leg supporting 
period to correct a feet compensating angle : whereas a foot floor inclination difference is estimated during the one-leg 
supporting period to correct a foot compensating angle. (Since the ankle joint angle is corrected in the technique 
mentioned in 6(1 994)-31 658, strictly speaking, it is different from the correction of the foot compensating angle. Nev- 
ertheless, they are treated as almost the same in this discussion.) 
45 [0041] It should be noted herethatthe ZMP (Zero Moment Point) is defined as follows: Namely, obtaining the resultant 
force of the force of inertia and the gravity generated by a motion pattern (trajectory), if the moment except for the 
vertical moment component, of the obtained resultant force about a certain point of action on the floor (floor surface) 
is zero 

[0042] On the contrary, in the floor shape estimation system of a legged mobile robot according to this embodiment, 
50 the floor shape, more specifically a foot-to-foot floor inclination difference is estimated based on at least a control error 
of a total floor reaction force's moment (or a difference obtained by subtracting another control error of foot floor reaction 
force's moment about a desired foot floor reaction force central point therefrom). Further, in this system, the feet com- 
pensating angle is corrected by the estimated value, if needed. Further, in this system, another floor shape, more 
specifically a foot floor inclination difference is estimated based on at least a control error of the foot floor reaction 
55 force's moment about the desired foot floor reaction force central point. Further, in this system, the foot compensating 
angle is corrected based on the estimated value, if needed. As a result, this system can accurately estimate the foot 
floor inclination difference during the two-leg supporting period and can correct the foot compensating angle by the 
estimated value, 
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[0043] Further, in the estimation of the foot-to-foot floor inclination difference and the foot floor inclination difference, 
by using a mechanism compliance model which models the compliance mechanism 42 and expresses the relationship 
of deformation of the leg (leg links) 2 by the floor reaction force, a better control performance can be obtained which 
would not be achieved in the prior art technique in which the steady-state error is decreased by incorporating integration 
5 into a control law. A mere incorporation of an integral value into a control law may result in a phase lag. However, in 
the system according to the embodiment, similar to a case in which an observer is used, no delay occurs in the control. 
[0044] Aside from the above, the prior art technique proposed in 1 0 (1 998)-277969 aims to restore a stable posture 
by controlling the floor reaction force appropriately, when the robot loses posture stabilization on a floor having an 
unexpected slant or bump. 

10 [0045] Specifically, if the robot (which is controlled to walk in accordance with a gait that has been generated based 
on the supposition that a floor on which the robot walks is flat) lands its foot on an unexpected bump, the robot is 
suffered from an excessive floor reaction force and tilts. In orderto solve this problem, the applicant proposed in another 
technique (described in Japanese Laid-Open Patent Application No. Hei 5 (1 993)-305586), using the aforesaid com- 
pliance mechanism 42, detecting the inclination of the body by the inclination sensor to calculate a required moment 

15 necessary for restoring the tilted posture and controlling each foot to move up and down such that an actual total floor 
reaction force's moment component about the desired total floor reaction force central point (i.e., the desired ZMP) 
becomes equal to the required moment. 

[0046] However, that control can effectively cope with an unexpected slant or undulation extending over a relatively 
wide area, but can not cope with an unexpected local slant or level difference sufficiently. 

20 [0047] Further, the applicant proposes in a still another technique (described In Japanese Laid-Open Patent Appli- 
cation No. Hei 5(1993)-305584)) controlling the ankle joints 18R(L), 20R(L) to move in a direction in which the unex- 
pected floor reaction force is canceled, similarly utilizing the aforesaid compliance mechanism 42. 
[0048] It would be possible to combine the two kinds of the controls to cope with the situation when the robot loses 
its stable posture due to an unexpected floor undulation or inclination. Disadvantageously, if doing so, the controls may 

25 conflict with each other. 

[0049] In order to avoid such a conflict in control, it is proposed in the technique proposed earlier (i.e., 10(1998)- 
277969) controlling the floor reaction force appropriately when the robot loses a stable posture due to the change in 
the shape of floor such as a local slant or inclination. 

[0050] Since the floor inclination estimation system according to the embodiment is based on the aforesaid technique 
30 proposed in 1 0(1998)-277969, for the ease of understanding, this technique (10-277969) will first be explained. This 
technique (10(1 998)-277969) will hereinafter referred to as a "composite compliance control". 

[0051] Figure 4 is a block diagram showing the configuration and operation of the system for the composite compli- 
ance control in afunctional manner. 

[0052] Before entering the explanation of the figure, terms used in the composite control will be defined. (Terms not 
35 defined in the following are the same as those defined in a separate application (Japanese Patent Application No. Hei 
10(1998)-86081) proposed by the applicant.) 

[0053] The term "gait" is used to indicate, different from that usually defined in the field of robotics engineering, what 
includes the desired motion pattern and the floor reaction force pattern. The floor reaction force may be partial infor- 
mation such as "only the ZMP trajectory". In other words, the term "gait generator" should not be used in the situation 

40 when only the desired motion pattern, and not the information concerning the floor reaction force pattern is outputted. 
[0054] Each of the feet is assigned with a reference numeral. The floor reaction force acting on n-th leg (n: 1 or 2) 
is called a "n-th foot floor reaction force" . The resultant force of all of the foot floor reaction forces acting both feet (legs) 
is called a "total floor reaction force". (Although, it is simply called the floor reaction force in the field of robotics engi- 
neering, it is called the "total floor reaction force" here in orderto distinguish from the (n-th) foot floor reaction force.) 

45 [0055] The foot floor reaction force is described by a point of action and the force and the moment of force acting 
thereat. A combination of descriptions will accordingly be innumerable for a certain floor reaction force. The innumerable 
combinations of descriptions will surely include the combination of descriptions indicating that the moment components 
other than that about the vertical axis are zero and the point of action is on the floor. The point of action in this combination 
of descriptions is called the "foot floor reaction force central point". (This was called "the center of contact-pressure" 

50 in Japanese Laid-Open Patent Application Hei 6 (1 994) - 79657 proposed by the applicant (referred to later)). 

[0056] Similarly, the total floor reaction force is described by a point of action and the force and the moment of force 
acting thereon. A combination of descriptions will accordingly be innumerable for a given total floor reaction force. The 
innumerable number of combinations of descriptions will surely include the combination of descriptions indicating that 
the moment components other than that about the vertical axis are zero and the point of action is on the floor surface. 

55 The point of action in this combination of descriptions is called the "total floor reaction force central point". The "floor 
surface" in this specification includes a vertical floor described in Japanese Laid-Open Patent Application No. Hei 5 
(1993-318340). 

[0057] A desired value of the total floor reaction force is called the "desired total floor reaction force". The desired 
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total floor reaction force is the total floor reaction force which normally balances dynamically with the desired motion 
pattern. Accordingly, the desired total floor reaction force central point is normally equal to the desired ZMP. 
[0058] A desired value of the ZMP (Zero Moment Point) is called a "desired ZMP (Zero Moment Point)". The desired 
ZMP can solely be determined unless the vertical force component of the resultant force is zero. The term "desired 
ZMP" is often used in this specification for ease of understanding. However, strictly speaking, the term "desired total 
floor reaction force central point" should instead be used. 

[0059] A desired value or the foot floor reaction force is called "desired foot floor reaction force". Different from the 
desired total floor reaction force, the desired foot floor reaction force can not be determined solely even when the 
desired motion pattern has been determined. Total floor reaction force actually acting on the robot is called an "actual 
total floor reaction force". Floor reaction force actually acting on each foot of the robot is called an "actual foot floor 
reaction force" 

[0060] Based on the above, the entire configuration of the composite compliance control system will be explained 
with reference to Figure 4. 

[0061] The composite compliance control system includes a gait generator 100 which generates the desired gait 
and outputs the same. As will be understood from the definition mentioned above, the desired gait comprises the 
desired motion pattern and the desired floor reaction force pattern, more specifically, a desired body position and/or 
posture trajectory, a desired feet position and/or posture trajectory, a desired total floor reaction force central point 
(desired ZMP) trajectory and a desired total floor reaction force trajectory (or pattern). Thus, the desired floor reaction 
force pattern includes the desired total floor reaction force central point trajectory. (It suffices if the desired floor reaction 
force pattern merely has the desired total floor reaction force central point trajectory, when deformation compensation 
(mechanism-deformation compensation), explained later, is not involved.) 

[0062] In this embodiment, the desired total floor reaction force outputted by the gait generator 1 00 is the total floor 
reaction force which dynamically balances with the desired motion pattern. Accordingly, the desired total floor reaction 
force central point is equal to the desired ZMP. 

[0063] Figure 5 shows one example of motion pattern when the robot 1 walks on a flat floor. Figure 6 shows the 
trajectory of the desired ZM P on the floor corresponding thereto, and Figure 7 shows a time chart corresponding thereto. 
The foot which is kept in contact with the floor during the illustrated gait period is to be named a "1st foot", while the 
other is to be named a "2nd foot". 

[0064] Returning to the explanation of Figure 4, the system includes a desired floor reaction force distributor 102 
which receives the desired total floor reaction force central point (desired ZMP) and the desired feet position and/or 
posture as main inputs, and determines and outputs the desired foot floor reaction force central point. Practically, if 
required, it may also input, from the gait generator 1 00, gait parameters (such as a period of time of two-leg supporting 
period and desired landing position of free leg foot) or a period of time and time of gait (such as the current time is 0.1 
sec. after the beginning of the two-leg supporting period). 

[0065] As regards the gait such as illustrated in Figure 5, the desired floor reaction force distributor 1 02 determines 
the desired foot floor reaction force central point such that it satisfies the following conditions: 

Condition 1 ) The desired foot floor reaction force central point trajectory is continuous. 

Condition 2) During the two-leg supporting period, the desired 1st foot floor reaction force central point exists at 
its heel, whereas the desired 2nd foot floor reaction force central point exists at its toe. 

Condition 3) The desired total floor reaction force central point exists, at that instant, on a line connecting the 
desired 1st foot floor reaction force central point and the desired 2nd foot floor reaction force central point. 
Condition 4) During the one-leg supporting period, the desired 1st foot floor reaction force central point is equal 
to the desired total floor reaction force central point. 

Condition 5) During the one-leg supporting period, the desired 2nd foot floor reaction force central point shifts from 
the toe to the heel. 

[0066] Figure 8 shows a time chart of the desired 1 st foot floor reaction force central point trajectory satisfying these 
conditions, whilst Figure 9 shows that of the 2nd foot floor reaction force central point trajectory. In the figures, the 
origin is defined as the point of foot, more precisely on a sole of the foot, projected from the ankle (joints 1 8R(L), 20R 
(L)) onto the sole of the foot 22R(L) at the angle perpendicular to the plane defined by the sole of foot, whilst the positive 
direction on the X-axis is defined as the direction from the origin towards the front of the foot and along the plane 
defined by the foot, and the positive direction on the Y-axis is defined as the direction to the left of the foot from the 
origin as shown in Figure 1 . 

[0067] Additionally, the desired floor reaction force distributor determines 1 02 outputs the desired foot floor reaction 
force. The desired foot floor reaction force is necessary for the deformation (bent) compensation (mechanism-defor- 
mation compensation) of the compliance mechanism 42. 

[0068] If the desired foot floor reaction force is determined to be corresponding to the desired foot floor reaction force 
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central point, determined in the manner mentioned above, using the following equation, it will satisfy a condition that 
the resultant force of all of the desired foot floor reaction forces of the feet must be equal to the desired total floor 
reaction force. 

Desired 1st foot floor reaction force = Desired total floor reaction force * 
(Distance between the desired 2nd foot floor reaction force central point and the desired 
ZMP)/(Distance between the desired 1st foot floor reaction force central point and the 
desired 2nd foot floor reaction force central point) 
Desired 2nd foot floor reaction force = Desired total floor reaction force * 
(Distance between the desired 1st foot floor reaction force central point and the desired 
ZMP)/(Distance between the desired 1st foot floor reaction force central point and the 

desired 2nd foot floor reaction force central point) Eq. 1 



[0069] Since the desired foot floor reaction force changes continuously, it is suitable for achieving a walking with 
small foot-landing impact. The details of the above is described in an application (Japanese Laid-Open Patent Appli- 
cation Hei 6 (1 994) - 79657) proposed by the applicant. 

25 [0070] Returning to the explanation of Figure 4, the system includes a posture stabilization controller 104 which 
estimates the conditions of the robot 1 based on the sensed information of the robot and calculates a compensating 
total floor reaction force. Explaining this, even if joint displacement can be controlled to effect a desired joint displace- 
ment completely by a displacement controller (explained later) when the robot 1 walks or stand upright, the robot 
position and/or posture will not always be a desirable position and/or posture. 

30 [0071] Accordingly, in order to stabilize posture of the robot 1 over a long period of time, it needs to determine the 
force and moment necessary for restoring desirable robot position and/or posture : and to additionally generate the 
determined force and moment at the desired total floor reaction force central point (desired ZMP) as the point of action. 
The additional force and moment is called a "compensating total floor reaction force". The moment component of the 
compensating total floor reaction force is called a "compensating total floor reaction force's moment". 

35 [0072] Aside from the above, assuming that a desired gait of the legged mobile robot is subject to a reaction force, 
other than the floor reaction force, from an object in the environment, naming this reaction force "reaction force from 
object", for example, the aforesaid definition of the desired ZMP may be extended as follows. That is; obtaining the 
resultant force of the force of inertia, the gravity and the reaction force from the object generated by the desired motion 
pattern, if this moment, except for the vertical component, of the obtained resultant force about a certain point of action 

40 on the floor surface is zero, the point of action may newly be defined as the "desired ZMP". 

[0073] Assuming that the robot 1 is completely rigid and joint displacement can be controlled to track desired joint 
displacement perfectly, a perturbative motion of the position and/or posture of entire of the robot due to deformation 
of the compliance mechanism 42 can be decomposed to the following six degrees of freedom: 

45 Mode 1) Rotation about the desired total floor reaction force centra! point (desired ZMP) around the back-and- 

forth-axis (I.e., tilting in the-left-and-right direction). 

Mode 2) Rotation about the desired total floor reaction force central point (desired ZMP) around the left-and-right- 
axis (i.e, : tilting in the back-and-forth direction). 

Mode 3) Vertical rotation about the desired total floor reaction force central point (desired ZMP) (i.e., spinning). 
50 Mode 4) Parallel-translational swinging in the back-and-forth direction. 

Mode 5) Parallel-translational swinging in the left-and-right direction. 
Mode 6) Parallel-translational swinging in the vertical direction. 

[0074] Among of these modes, the swingings of Mode 4 and Mode 5 occur with the deformation of the compliance 
55 mechanism 42 when it suffers from shearing force in the back-and-forth and left-and-right directions. Since the com- 
pliance mechanism 42 is prefabricated to have high stiffness in the direction of shearing, this kind of swinging is small, 
influencing on the walking little. 

[0075] Since Mode 3 and Mode 6 do not have direct relationship with this explanation controls for Mode 1 and Mode 
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2 will be discussed. The controls for Mode 1 and Mode 2 are quite important, since, but for these controls, the robot 
may probably tilt. 

[0076] The manipulated variable for controlling the rotation of mode 1 is a moment component about the back-and- 
forth axis (X-axis) of the compensating total floor reaction force. The manipulated variable for controlling the rotation 

s of Mode 2 is the moment component about the left-and-right axis (Y-axis) of the compensating total floor reaction force. 
Among the moment components of the compensating total floor reaction force, accordingly, it suffices if the moment 
component about the back-and-forth axis and that about the left-and-right axis are determined. Since the rest of the 
moment components are not used in this embodiment (and a second embodiment), they are set to zero. 
[0077] From now on, the following definitions will be used. The moment component of compensating total floor re- 

f o action force is called a "compensating total floor reaction force's moment Mdmd" (more precisely, compensating total 
floor reaction force's moment Mdmd about the desired total floor reaction force centra! point (desired ZMP)). As illus- 
trated in Figures 5 and 6, a coordinate system taking the X-axis in the robot forwarding direction, the Y-axis in the robot 
left side direction and the Z-axis in the vertical direction and having the origin which is set at a point on the floor surface 
immediately below the 1st foot ankle, is called a "supporting-leg coordinates system". Unless otherwise determined, 

<5 the position, the force and the moment are expressed in accordance with this coordinate system. X-component of 
Mdmd is named a "Mdrndx", Y-component thereof is named a "Mdmdy" and Z-component thereof is named a "Mdmdz". 
X-component of an inclination error 0 err of the body 24 (i.e., actual body inclination - desired body inclination) is named 
"Gerrx'V Y-component thereof is named a "Gerry", and their time differentials are named "(d eerrx/dt)" and "(dG erry/dt)". 
[0078] Mdmdx and Mdmdy are determined in accordance with, for example, a control law mentioned in the following 

20 equation: 

Mdmdx =- Kthx Gerrx - Kwx (dGerrx/dt) 
25 Mdmdy = - Kthy 9 erry - Kwy (d6 erry/dt) Eq. 2 

[0079] In the above, Kthx, Kthy, Kwx and Kwy are control gains for body inclination stabilization. 
[0080] The composite compliance operation determinator acts such that the actual total floor reaction force becomes 
equal to the resultant force of the desired total floor reaction force and the compensating total floor reaction force, as 
30 will be explained later. 

[0081 ] Returning to the explanation of Figure 4, the system includes an actual foot floor reaction force detector 1 08 
which detects the actual foot floor reaction force through the output of the force sensor 34. (The resultant force of the 
sum of the foot floor reaction forces is the actual total floor reaction force.) Moreover, based on the actual joint dis- 
olacements obtained from the joint rotary encoders (or based on the joint displacement commands), the actual foot 
35 floor reaction force detector calculates each foot position and/or posture relative to the coordinate system fixed on the 
body 24, then conducts coordinate-transformation of the detected values obtained by the force sensor 34 to calculate 
the actual foot floor reaction force in the coordinate system fixed at the body, and converts the calculated values into 
those in the supporting leg coordinate system. 

[0082] The system includes a robot geometric model (inverse kinematic solution) 110, which, upon receipt of the 
40 body position and/or posture and the feet position and/or posture, calculates respective joint displacements satisfying 
them. Since the degrees of freedom per leg are 6 in the robot 1 in this embodiment, the respective joint displacements 
are determined solely. 

[0083] In this configuration, equations of the inverse kinematic solution are directly prepared beforehand such that 
the respective joint displacements are immediately obtained if the body position and/or posture and the foot position 
45 and/or posture are put in the equations. More specifically, the robot geometric model 1 0 inputs the desired body position 
and/or posture and a corrected desired foot position and/or posture trajectory (a corrected desired feet position and/ 
or posture with deformation compensation) corrected by the composite compliance operation determinator, and cal- 
culates joint displacement commands for the 12 joints (including 10R(L), etc.). 

[0084] The system includes a displacement controller 112 (corresponding to the aforesaid 2nd calculator 62), which 
50 controls displacements of the 1 2 joints of the robot 1 to track or follow the joint displacement commands calculated by 
the robot geometric model (inverse kinematic solution) 110. 

[0085] The system includes the aforesaid composite compliance operation determinator 114, which corrects the 
desired foot position and/or posture trajectory to satisfy the following two demands: 

55 Demand 1 ) In orderto control robot position and/or posture, the actual total floor reaction force should be controlled 

to track the resultant force of the compensating total floor reaction force (i.e., the moment Mdmd) outputted by the 
posture stabilization controller 104 and the desired total floor reaction force. If only the robot posture inclination 
should be controlled, the horizontal direction's moment component of the actual total floor reaction force about the 
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desired total floor reaction force central point should solely be controlled to track the compensating total floor 
reaction force's moment Mdmd. 

Demand 2) In order to ensure the floor-contactability of each foot, the absolute value of the actual foot floor reaction 
force's moment about the desired foot floor reaction force central point should be decreased as much as possible. 

5 

[0086] Incidentally, in most situations, it is physically impossible to bring each of the actual moment of foot floor 
reaction force about the desired foot floor reaction force central point to be zero, while causing the actual total floor 
reaction force to be equal to the resultant force of the compensating total floor reaction force and the desired total floor 
reaction force. It will accordingly be difficult to completely satisfy both Demand 1 ) and Demand 2), and some compro- 

10 mise should therefore be needed. 

[0087] Based on the above, the operation of the system will be explained with reference to the flow chart (structur- 
alized flow chart) of Figure 10. The left of the flow chart indicates the elements illustrated in Figure 4 which conduct 
the procedures corresponding to those mentioned in the steps mentioned at the right of the flow chart. 
[0088] The program begins in S10 in which the system is initialized and proceeds, via S12, to S14 in which timer 

15 interrupt is waited for. The timer interrupt is executed once every 50 msec. Thus, the control cycle is 50 msec. 

[0089] Then, the program proceeds to S1 6 in which it is determined whether it is at gait switching, more specifically, 
it is determined whether it is at switching of supporting leg. if the result is negative, the program proceeds to S22. If 
the result is affirmative, the program proceeds to S18 in which a timer t is initialized, and to S20 in which the desired 
gait parameters are set. As mentioned above, the gait parameters comprise the motion parameters and the floor re- 

20 action force parameters (i.e., the ZMP trajectory parameters). 

[0090] The program proceeds to S22 in which instantaneous values of the desired gait are determined. Here, the 
"instantaneous values" indicate values at each control cycle. The instantaneous values of the desired gait comprise 
the desired body position and/or posture, the desired foot position and/or posture, and the desired ZMP position. Here, 
the "posture" indicates "direction or orientation" in the X, Y, Z space or environment. 

25 [0091] The program then proceeds to S24 in which the desired foot floor reaction force central point is determined. 
This is done in the manner explained with reference the desired floor reaction force distributor. Specifically, this is done 
oy determining the value of the set desired foot floor reaction force central point trajectory at the current time t, as 
illustrated in Figures 8 and 9. 

[0092] The program then proceeds to S26 in which the desired foot floor reaction force of each foot is determined. 
30 This is done by calculating the desired foot floor reaction force using Eq. 1 , mentioned in the explanation of the desired 
floor reaction force distributor. 

[0093] The program then proceeds to S28 in which the conditions of the robot such as the inclination of the body 24 
are detected using the outputs of the sensors including the inclination sensor 36. 

[0094] The program then proceeds to S30 in which the compensating total floor reaction force's moments Mdmdx, 
35 Mdmdy (about the desired total floor reaction force central point (desired ZMP)) for posture stabilization are determined 
oased on the detected conditions, etc. More specifically, the compensating total floor reaction force's moments Mdmdx, 
Mdmdy are determined in accordance with Eq. 2. 

[0095] The program then proceeds to S32 in which the actual foot floor reaction force is detected for each foot. This 
is done from the output of the force sensor 34, as mentioned above. 
40 [0096] The program then proceeds to S34 in which a feet compensating angle Odbv and a foot compensating angle 
G nx(y) for each foot are determined. This procedure is done by the aforesaid composite compliance operation deter- 
minator 114. 

[0097] The procedures to be done by the composite compliance operation determinator 114 will be explained. For 
ease of explanation, it is assumed that it is in a situation where the actual foot floor reaction force acts respectively on 
45 the 1st foot 22R(L) and the 2nd foot 22L(R) in the two-leg supporting period, as illustrated in Figure 11 . 

[0098] Here, a vector Fnact indicates a force component of the n-th foot floor reaction force, and a vector Mnact 
indicates a moment component of the n-th foot floor reaction force. The direction of vector Mnact indicates that the 
moment acts from the floor to the foot clockwise with respect to the vector direction. 

[0099) The desired total floor reaction force at this instant is assumed to be as shown in Figure 12. Notably, a vector 
so Msumref indicative of a desired total floor reaction force's moment about the desired total floor reaction force central 
point (desired ZMP) is vertical. (This is because the desired ZMP is a floor surface point at which the horizontal com- 
ponent of desired total floor reaction force's moment is zero from the definition mentioned above.) 
[0100] When the desired total floor reaction force is distributed to or divided into the respective desired foot floor 
reaction forces in accordance with Eq. 1 , the result will be that as shown in Figure 13. In the figure, a vector Fnref 
55 indicates a force component of the desired n-th foot floor reaction force. A vector Mnref indicates the moment compo- 
nent of desired n-th foot floor reaction force. The expression of the direction of vector Mnref is the same as that of Mnact. 
[0101] For ease of explanation, it is assumed that the robot is about to turn over backward to the left. 
[0102] The aforesaid posture stabilization controller 1 04 calculates the compensating total floor reaction force's mo- 
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ment Mdmd based on the detected body inclination errors 0errx, Gerry of the robot 1 . In this embodiment, since the 
spinning about the vertical axis (Z-axis) is not controlled, the vertical component of the compensating total floor reaction 
force's moment Mdmd is zero. Figure 14 shows the compensating total floor reaction force's moment Mdmd for the 
assumed condition 

5 [0103J In order to restore the posture, it suffices if the horizontal component of the actual total floor reaction force's 
moment about the desired total floor reaction force central point (desired ZMP) is controlled to track a horizontal com- 
ponent of the sum of the desired total floor reaction force's moment Msumref and the compensating total floor reaction 
force's moment Mdmd. 

[0104] The horizontal component of desired total floor reaction force's moment Msumref is zero at the desired total 
10 fioor reaction force central point (desired ZMP). Accordingly, in order to restore the posture inclination in the back-and- 
force or left-and-right direction, it suffices if the horizontal component of the actual total floor reaction force's moment 
about the desired ZMP is controlled to track the horizontal component of Mdmd. 

[0105] In this embodiment, the composite compliance operation determinator 114 corrects the foot position and/or 
posture so as to satisfy the following demands (once mentioned in the above) as much as possible: 

15 

Demand 1) In order to conduct stabilization-control of the robot posture inclination, the horizontal components (X- 
axis and Y-axis) of the actual total floor reaction force's moment about the desired total floor reaction force central 
point (desired ZM P) should be controlled to track the horizontal components of the compensating total floor reaction 
force's moment Mdmd. 

20 Demand 2) In order to ensure the floor-contactability of each foot, the absolute value of the actual foot floor reaction 

force's moment about the desired foot floor reaction force central point should be decreased as much as possible. 

[0106] As mentioned above, it is impossible to satisfy both Demand 1)and Demand 2) and hence, some compromise 
is needed. 

25 [0107] In this embodiment, the correction of foot position and/or posture is conducted as follows. 

1) A vector V which is normal on the plane that includes the desired 1st foot floor reaction force central point Q1 
and the 2nd foot floor reaction force central point Q2 and that is perpendicular to the horizontal plane, is first 
determined. The magnitude of the vector V should be 1 . Figure 1 5 shows the vector V. 

30 2) The coordinate of the desired 1st foot floor reaction force central point Q1 is rotated (moved) by a rotational 

angle 0 dbv about the desired total floor reaction force central point (desired ZMP) around the normal vector V. 
The central point after rotation is called "Q1 m . Similarly, the coordinate of the desired 2nd foot floor reaction force 
central point Q2 is rotated (moved) by the same rotational angle 0 dbv about the desired total floor reaction force 
central point (desired ZMP) around the normal vector V. The central point after rotation called" Q2"\ 

35 This rotational angle 0 dbv is called the "feet compensating angle". The vector whose initial point is Q1 and 

whose terminal point is Q1\ is named a "vector Q1Q1'". Similarly, a vector whose initial point is Q2 and whose 
terminal point is Q2', is named a "vector Q2Q2"\ Figure 16 shows Q1 f and Q2*. 

3) Then, the desired 1st foot is parallel-translated (moved up and down) by the vector Q1Q1V without changing 
its posture. Similarly, the desired 2nd foot is parallel-translated by the vector Q2Q2\ without changing its posture. 

40 Figure 1 6 shows each desired foot after movement with thick lines. 

4) Then, the desired 1st foot is rotated about Q1\ by a rotational angle 01 x around the back-and-forth axis (X-axis) 
and by a rotational angle 01 y around the left-and-right axis (Y-axis). Similarly, the desired 2nd foot is rotated about 
Q2', by a rotational angle 6 2x around the back-and-forth axis (X-axis) and by a rotational angle 02y around the 
left-and-right axis (Y-axis). The rotational angles 0nx and 0ny are called an "n-th foot X-compensating angle" and 

45 an "n-th foot Y-compensating angle", respectively. Figure 17 shows each desired foot after rotation with thick lines. 

[01 08] If the amount of compensating operation mentioned above is not excessive, the contact-area of foot sole (the 
foot sole area where the pressure is positive) remains unchanged, although the distribution of pressure of the foot sole 
may change. If this is the case, the compliance mechanism 42 provided at each foot deforms by an amount proportionate 
50 to the amount of compensating operation and generates the actual foot floor reaction force at each foot in response 
to the amount of deformation. With this, the relationship between the amount of compensating operation and the change 
in the actual foot floor reaction force generated by the compensating operation will exhibit the following appropriate 
characteristics: 

55 Characteristic 1 ) If only the feet compensating angle 0 dbv is manipulated to move the desired feet position, the 

actual foot floor reaction force's force component of the one foot which is moved down, increases, whilst that of 
the foot which is moved up, decreases. At this time, the actual foot floor reaction force's moment about the corrected 
desired foot floor reaction force central point changes little. 
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Characteristic 2) If only the n-th foot X-compensating angle is manipulated to rotate the n-th foot posture, only the 
X-component of the actual n-th foot floor reaction force's moment about the desired n-th foot floor reaction force 
central point changes, but the other floor reaction force components change slightly. Similarly, if only the n-th foot 
Y-compensating angle is manipulated to rotate the n-th foot posture, only the Y-component of the actual n-th foot 
floor reaction force's moment about the desired n-th foot floor reaction force central point changes, but the other 
floor reaction force components change slightly. 

Characteristic 3) When the feel compensating angle Odbv, and the foot X-compensating angle and the foot Y- 
compensating angle are all manipulated at the same time, the change amounts of actual foot floor reaction force 
will be the sum of the individual change amounts obtained by respective manipulation. 



[0109] It could be stated from the above that characteristics 1) and 2) indicate that the respective manipulation have 
independence, whiist characteristic 3) indicates that the respective manipulations have linearity. 
[0110] Figure 18 is a block diagram showing the calculation of the composite compliance operation determinator 
114. The calculation will be explained with reference to the figure. 

75 [01 11 ] Briefly explaining, a total floor reaction force compensating moment distributor 1 1 4a distributes or divides the 
compensating total floor reaction force's moment Mdmd. Then, a feet compensating angle determinator 114b and n- 
th foot X-compensating and Y-compensating angle determinators 114c, 114d, 114e, 114f determine the aforesaid com- 
pensating angles 6 dbv and e nx (y) based on the actual foot floor reaction force and the distributed compensating total 
floor reaction force's moment, etc. 

20 [01 1 2] Then , a corrected desired foot position and/or posture calculator 1 14g determines a compensated feet position 
and/or posture (named as a "corrected desired feet position and/or posture") through geometric caiculation based on 
these determined compensating angles. Finally, a corrected desired feet position and/or posture with deformation 
compensation calculator 1 1 4h determines the amount of deformation of the compliance mechanism 42 presumably 
generated by the desired foot floor reaction force, and further corrects the corrected desired feet position and/or posture 

25 to cancel the same. 

[01 1 3] To be more specific, the total floor reaction force compensating moment distributor 1 1 4a distributes the com- 
pensating total floor reaction force's moment Mdmd to (divides into) a feet compensating moment Mdmddb and foot 
compensating moments Mdmdlx, y, Mdmd2x, y. The feet compensating moment Mdmddb is a desired value of moment 
to be generated about the desired total floor reaction force central point (desired ZMP) by the force component of the 
so foot floor reaction force through the manipulation of the feet compensating angle (the amounts of feet up-and-down 
movement) 0 dbv. 

[0114] The component around the V-direction of the feet compensating moment Mdmddb is expressed as Mdmddbv. 
The V-direction is the direction of the vector V defined with reference to the explanation of the composite compliance 
operation determinator. Defining a vector which is perpendicular to V and also the vertical direction, as "U", a U-direction 
35 component Mdmddbu of the feet compensating moment Mdmddb is set to zero. This is because the manipulation of 
feet compensating angle 0 dbv can not generate the U-direction moment of floor reaction force. 
[01 1 5] Here, since the vertical component of the compensating total floor reaction force's moment Mdmd is zero, the 
vertical component Mdmddbz of Mdmddb is also set to zero. 

[0116] The 1st foot compensating moment Mdmdl is a moment to be generated about the desired 1st foot floor 
40 reaction force centra! point by manipulating the 1st foot compensating angles 01 x, 01 y. The X-component of 1st foot 
compensating moment Mdmdl is expressed as Mdmdlx, whiist the Y-component thereof is expressed as Mdmdl y. 
The 2nd foot compensating moment Mdmd2 is a moment to be generated about the desired 2nd foot floor reaction 
force central point by manipulating the 2nd foot compensating angles 62x, 62y. The X-component of 2nd foot compen- 
sating moment Mdmd2 is expressed as Mdmd2x, whilst the Y-component thereof is expressed as Mdmd2y. 
45 [0117] The distribution will be carried out, for example, in the manner mentioned below. 



10 



Mdmddbv = Wdbx * Mdmdx+ Wdby * Mdmdy 



Eq. 3 



50 



Mdmdlx 



W1x * (Mdmdx - Wint * Vx * Mdmddbv) 



Mdmdl y 



W1y * (Mdmdy - Wint * Vy * Mdmddbv) 



55 



Mdmd2x 



W2x * (Mdmdx - Wint * Vx * Mdmddbv) 



Mdmd2y 



W2y * (Mdmdy - Wint * Vy * Mdmddbv) 



Eq. 4 
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[0118] In the above, Wdbx, Wdby, W1x, W1y, W2x, W2y and Wint are distribution-weight variables. Vx is a value of 
the X-component of vector V and Vy is that of Y-component of vector V. Among of the variables, Wint is a variable for 
canceling the total floor reaction force's moment (generated by the manipulation of the feet compensating angle) by 
manipulating the foot compensating angles. 

[0119] Figure 19 is a block diagram showing the total floor reaction force compensating moment distributor 114a 
which conducts the calculation of Eqs. 3 and 4. 

[0120] Figure 20 shows an example of the distribution-weight variables Wdbx, Wdby, W1x, W1y, W2x, W2y and 
Wint, during robot walking. The patterns illustrated in Figure 20 should preferably be set taking the following attentions 
into account: 

Attention 1 ) If the feet compensating angle and the foot compensating angle vary discontinuously, the joints ex- 
perience excessive torque. In order to change the feet compensating angle and the foot compensating angle 
continuously, the distribution-weight variables should be changed continuously. 

Attention 2) The distribution-weight variables should be determined such that the actual floor reaction force's mo- 
ment generated by the manipulation of the feet compensating angle and the foot compensating angle, is preferably 
close to the compensating total floor reaction force's moment Mdmd. 

Moreover, the variables should preferably be set in response to the motion of robot such as being upright, 
walking, etc. In a situation such as being upright in which the V-direction-components Mdmddbv of the feet com- 
pensating moment and the foot compensating moment Mdmdl , Mdmd2 can be faithfully generated in the actual 
foot floor reaction force, the variable should preferably be set as follows. 

Specifically, in this situation, the variables should be set, satisfying both Eqs. 5 and 6 as much as possible, 
such that the horizontal component of the actual total floor reaction force's moment about the desired total floor 
reaction force central point becomes equal to the horizontal component of the compensating total floor reaction 
force's moment Mdmd (i.e. : Demand 1) to the composite compliance operation determinator can be satisfied. 

Mdmddbv * Vx + Mdmdl x+ Mdmd2x = Mdmdx Eq. 5 

Mdmddbv * Vy + Mdmdl y + Mdmd2y = Mdmdy Eq. 6 

Substituting Eqs. 5 and 6 into Eqs. 3 and 4 respectively, Eq. 5 is rewritten as Eq. 7, while Eq. 6 is rewritten as 
Eq. 8 

(Wdbx * Mdmdx+ Wdby * Mdmdy) * Vx + W1x * (Mdmdx - Wint * Vx * 
(Wdbx * Mdmdx + Wdby * Mdmdy)) + W2x * (Mdmdx - Wint * Vx * (Wdbx * 

Mdmdx+ Wdby * Mdmdy)) = Mdmdx Eq. 7 



(Wdbx * Mdmdx + Wdby * Mdmdy) * Vy + W1y * (Mdmdy - Wint * Vy * 
(Wdbx * Mdmdx + Wdby * Mdmdy)) + W2y * (Mdmdy - Wint * Vy * (Wdbx * 

Mdmdx +■ Wdby * Mdmdy)) = Mdmdy Eq. 8 

In order to always satisfy Eqs. 7 and 8. no matter what values Mdmdx and Mdmdy are, it suffices if Eqs. 9, 1 0 
and 11 are satisfied at the same time. 

Wint =1 Eq. 9 



W1x+W2x=1 Eq. 10 
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W1y + W2y-1 Eq. 11 

In the situation mentioned above, thus, it suffices if the variables are determined to satisfy Eqs. 9, 10 and 11 
5 at the same time. 

When the robot is walking, the generated actual total floor reaction force's moment may sometimes be smaller 
than Mdmddbv, even if the feet position are corrected by manipulating the feet compensating angle 0dbv so as to 
generate Mdmddbv. As shown in Figure 21 , for example, assuming that the robot tilts backward such that the 1 st 
foot has not landed at the beginning of the two-leg supporting period, the actual floor reaction force does not 

to change even if the 1 st foot is moved down by Gdbv. 

Similarly, the increase of actual floor reaction force's moment may sometimes be smaller than Mdmd2, even 
if the angle of the 2nd foot is corrected by manipulating the 2nd foot compensating angle 02 so as to generate 
Mdmd2. As shown in Figure 22, for example, assuming that the robot tilts backward at the latter half of the two- 
leg supporting period, the actual floor reaction force does not change even if the 2nd foot heel is moved down. 

75 Thus ; even when the weight variables are determined to satisfy Eqs. 5 and 6, the increase of the actual floor 

reaction force generated by the composite compliance control may sometimes be less than the necessary com- 
pensating total floor reaction force's moment Mdmd. The values obtained by dividing the left terms of Eqs. 5 and 
6 by their respective right terms should accordingly be set to be greater than 1 under the situation where this 
problem would be likely to occur. 

20 in Figure 20 showing an example of setting of the distribution-weight variables during walking, Wint is set to 

zero such that the manipulation of foot compensating angle can make up forthe lack, even when the manipulation 
of the feet compensating angle edbv can not generate the actual total floor reaction force's moment in the situation 
such as illustrated in Figure 21 . 

Advantageously, if the robot tilts backward as illustrated in Figure 21 , since the 2nd foot heel moves down to 

25 be easily landed on the floor, the actual total floor reaction forces's moment can easily be generated by manipulating 

the 2nd foot compensating angle. 

When the robot is not tilting backward, the actual total floor reaction force's moment is generated by the ma- 
nipulation of the feet compensating angle G dbv, but the 2ndfoot heel is not in contact with the floor, the manipulation 
of 2nd foot compensating angle does not produce the actual total floor reaction force's moment. 

30 More specifically, the foot compensating angle does not act effectively when the feet compensating angle 0 

dbv acts effectively, whilst the feet compensating angle 6 dbv does not act effectively when the foot compensating 
angle acts effectively. The sum of the respective actual total floor reaction force's moments generated by manip- 
ulating the feet compensating angle and the foot compensating angle becomes, as a result, almost equal to the 
compensating total floor reaction force's moment Mdmd. 

35 Depending on the situation, the sum of the respective actual total floor reaction force's moments generated 

by manipulating the feet compensating angle and the foot compensating angle may sometimes become greater 
than the compensating total floor reaction force's moment Mdmd. 

Even if this happens, as Mdmd is a feedback manipulated variable for posture stabilization, it will not bring a 
serious problem, since, as will often be experienced in every control system, the magnitude variance of Mdmd will 

40 cause the open-loop gain in the control system to vary slightly, but the closed-loop characteristics do change little. 

Attention 3) The distribution-weight variables for feet compensating angle, Wdbx, Wdby, should be small (in ab- 
solute value) during the one-leg supporting period. This is because to change the feet compensating angle during 
the one-leg supporting period does only result in a wasteful up-and-down movement of a free foot and does not 
change the actual foot floor reaction force. 

45 Attention 4) If the force component of the desired foot floor reaction force is small (in absolute value), the distribu- 

tion-weight variable for the foot compensating angle of the foot concerned, should be small (in absolute value) so 
as to ensure the floor-contactability of the foot. In particular, if the foot is far from the floor, the absolute value of 
the distribution-weight variable should be small, since the movement of the foot by the foot compensating angle 
does not result in the change of the actual floor reaction force of that foot, so as to avoid a wasteful foot movement. 

50 Attention 5) The direction of the actual total floor reaction force's moment controlled by manipulating the feet 

compensating angle is ordinarily different from that controlled by manipulating the foot compensating angle. 

[0121] For example, the direction of the actual total floor reaction force's moment generated by manipulating the feet 
compensating angle Gdbv is always the V-direction, and it can not generate any component perpendicular to the V- 
55 direction. On the contrary, the direction of the actual total floor reaction force's moment generated by manipulating the 
foot compensating angle is restricted depending on a situation how the foot is in contact with the floor 
[0122] For example, if the foot is in contact with the floor only by the toe edge or heel edge of the foot 22R(L), no 
moment can be generated in the direction of the line of the edge. During the two-leg supporting period, taking this 
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characteristic into account, the feet compensating angle and the foot compensating angle should accordingly be ma- 
nipulated with minimum loss. 

[0123] For example, the distribution-weight variables Wdbx, Wdby for manipulating the feet compensating angle 
should be determined as follows. 

5 [0124] Specifically, defining a vector, as "Wdb", whose X-component is Wdbx, whose Y-component is Wdby and 
whose Z-component is zero, Eq. 3 is the inner product of the vector Wdb and Mdmd. It can therefore be said that what 
is obtained by dissolving Mdmd to a component of the direction of vector Wdb and a component perpendicular thereto 
and then by extracting only the component of the direction of vector Wdb and by multiplying by the magnitude of vector 
Wdb, is Mdmddbv calculated in accordance with Eq. 3. 

10 [0125] Figure 23 shows this Mdmddbv. This indicates that afeedback control system which controls the Wdb-direction 
component of the actual total floor reaction force's moment is built by manipulating the feet compensating angle. If the 
Wdb-diroction is perpendicular to the vector V } the Wdb-direction component of the actual total floor reaction force's 
moment will not produce no matter to wh at extent the feet compensating angle is manipulated, and the feedback control 
system would accordingly control the feet compensating angle in vain. 

15 [0126] In order to decrease such a wasteful operation, the Wdb-direction should accordingly be equal to the direction 
of vector V or be brought to be close thereto as much as possible. Moreover, when it is intended to produce the Wdb- 
direction component of the compensating total floor reaction force's moment, without using the foot compensating 
angle, but by solely manipulating the feet compensating angle, the inner product of Wdb and V should be determined 
to be 1 . If it is intended to generate a part of the component by the foot compensating angle, the inner product of Wdb 

20 and V should be determined to be less than 1 . 

[0127] Aside from the above, when the lateral width of the foot is small, the X-component of the actual foot floor 
reaction force's moment generated by manipulating the foot compensating angle will be small. In this case, the mag- 
nitude of Wdbx should preferably be set to be larger. With this, although the Wdb-direction and the direction of the 
vector V become not equal to each other and the fluctuation of the feet compensating angle increases, the stability will 

-?5 be enhanced. 

[0128] The processing of the feet compensating angle determinator 114b will be explained further. Figure 24 is a 
block diagram showing the calculation of the feet compensating angle determinator 114b. The feet compensating angle 
6 dbv is calculated in the manner as illustrated. 

[0129] Explaining this with reference to Figure 24, a moment Mf1f2act, which is generated about the desired total 
30 floor reaction force central point P by F1act acting on the desired 1st foot floor reaction force central point Q1 and by 
F2act acting on the desired 2nd foot floor reaction force central point Q2, is determined in accordance with the following 

equation: 

35 Mf1f2act = PQ1 * F1act + PQ2 * F2act Eq. 12 

[0130] In the above, PQ1 is a vector whose initial point is P and whose terminal point is Q1 , whilst PQ2 is a vector 
whose initial point is P and whose terminal point is Q2. 

[0131] Instead of Eq. 12, the following equation can be used without introducing any substantial problems: 

40 

Mf1f2act = PQ1 * F1act + PQ2 * F2act + M1act + M2act Eq. 12a 

[0132] Eq. 12a is an equation which calculates the actual total floor reaction force's moment Msumact acting about 
<*5 the desired total floor reaction force central point. Eq. 12 indicates the difference obtained by subtracting the actual 
foot floor reaction force's moment acting about the desired foot floor reaction force central point from the actual total 
fioor reaction force's moment acting about the desired total floor reaction force central point. 

[0133] Next, a vector V-direction-component Mf1f2actv of Mf1f2act is extracted. This can be obtained in accordance 
with the following equation which uses the calculation of vector inner product. The vector V is the vector illustrated in 
50 Figure 15 with reference to the aforesaid explanation. 

Mf1f2actv - Mf1f2act * V Eq. 13 

55 [0134] Then, Mf1f2act is filtered through a low-pass filter 114i to obtain Mf1f2actvfilt. 

[01 35] Then . the V-direction-component of feet compensating moment Mdmddbv is passed through a filter 1 1 4j for 
compensation and the filtered result is subtracted from Mf 1 f2actvf ilt to determine a V-direction-component of difference 
moment Mdiffv. 
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[01 36] The filter 1 1 4j for compensation is a filter for improving the frequency-response characteristic of the transfer 
function from Mdmddbv to the actual total floor reaction force's moment. 

[0137] Then, a feet deformation compensating angle e ffdbv for canceling the influence of the deformation of the 
compliance mechanism 42 on the V-direction-component of feet compensating moment is calculated. This corresponds 
to the so-called feedforward compensation. 

[0138] Specifically, this is done by using a mechanism-compliance model expressing the relationship between the 
V-direction-componeni of feet compensating moment Mdmddbv and the amount of deformation, by calculating the 
angle of deformation of the line connecting the desired 1st foot floor reaction force central point Q1 and the desired 
2nd foot floor reaction force central point Q2, and by inverting the polarity of the angle to determine the same as a feet 
deformation compensating angle G ffdbv. 

[0139] The feet deformation compensating angle G ffdbv can be approximated using the following equation: 

effdbv = -a* Mdmddbv Eq. 14 

[0140] In the above, a is a predetermined constant. 

[0141] Finally, the feet compensating angle Bdbv is obtained in accordance with the equation mentioned in the fol- 
lowing. In the following, Kdb is a control gain and is normally set to be a positive value. 

Odbv - Kdb * Mdiffv + effdbv Eq. 15 

[0142] The n-th foot compensating angle determinator will be explained. Figure 25 is a block diagram showing the 
calculation of a 1st foot X-compensating angle determinator 114c of the determinator. The 1st foot X-compensating 
angle determinator calculates the 1 st foot X-compensating angle 91 x in the manner illustrated. Although the illustration 
is omitted, the 1 st foot Y-compensating angle G1 y, the 2nd foot X-compensating angle G2x and the 2nd foot Y-compen- 
sating angle G2y are calculated similarly. Only the algorithm to determine the 1st foot X-compensating angleG 1 x will 
be explained here. 

[01 43] The X-component of the 1 st foot floor reaction force's moment M 1 actx is filtered through a low-pass filter 1 1 4k 
to obtain Mlactfiltx. The X-component of the 1st foot compensating moment Mdmdlx is passed through a filter 114m 
for compensation and the filtered result is subtracted from Mlactfiltx to obtain a difference moment Mdifflx. Similarly 
to the case of feet compensating angle determination, the filter 114m for compensation is a filter for improving the 
frequency-response characteristic of the transfer function from Mdmdl x to the actual floor reaction force. 
[0144] Then, similarly to the case of feet compensating angle determination, a 1st foot X-deformation compensating 
angle Gff 1 x for canceling the influence of the deformation of the compliance mechanism 42 on the X-component of 1 st 
foot compensating moment, is determined. This is the so-called "feedforward compensation". 

[0145] Specifically, this is done by using a mechanism-compliance model describing the relationship between the 
X-direction component of 1st foot compensating moment Mdmdlx and the amount of deformation, by calculating the 
angle of deformation of the 1 stfoot, and by inverting the polarity of the angle to determinethe same as a feet deformation 
compensating angle Gfflx. 

[0146] The feet deformation compensating angle Off 1 x can be approximated using the following equation: 

Gfflx = -a1x* Mdmdlx Eq. 16 

[0147] In the above. a1x is a predetermined constant. 

[0148] Finally, the 1st foot X-compensating angle G1x is obtained in accordance with the following equation. In the 
oelow, K1x is a control gain and is normally set to be a positive value. 

01x - K1x * Mdifflx + Gfflx Eq. 17 

[0149] Any equivalent alteration such as altering the order of calculation may be made on the block diagrams illus- 
trated. 

[0150] Returning to the explanation of Figure 18 and continuing the explanation thereon, the corrected desired feet 
position and/or posture calculator 114g corrects the desired feet position and/or posture, in accordance with the feet 
position and/or posture correction which is a development of the aforesaid composite compliance operation, based on 
the aforesaid feet compensating angle Bdbv, the 1st foot X-compensating angle G 1x, the 1st foot Y-compensating 
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angle 6 1y, the 2nd foot X-compensating angle Q2x and the 2nd foot Y-compensatlng angle 92y to determine the 
corrected desired feet position and/or posture. 

[0151] A deformation amount calculator 11 4n calculates the amount of deformation of the compliance mechanism 
42 which is expected to be generated by the desired foot floor reaction force. 
5 [0152] The corrected desired feet position and/or posture with deformation compensation calculator 114h further 
corrects the corrected desired feet position and/or posture so as to cancel the calculated amount of deformation, and 
determines the corrected desired feet position and/or posture with deformation compensation. 

[0153] For example, when the amount of deformation such as shown in Figure 26 is calculated, the corrected desired 
feet position and/or posture with deformation compensation is determined to be the position and/or posture as illustrated 

w in Figure 27 in thick lines. In other words, the calculator calculates the corrected desired feet position and/or posture 
with deformation compensation such that the position and/or posture of the deformation-compensated foot (illustrated 
in Figure 27) after having deformed by the desired foot floor reaction force, becomes equal to the foot position and/or 
posture before deformation compensation when no floor reaction force is acting (illustrated in Figure 26). 
[0154] The deformation compensation is a control which cancels a deviation of the actual foot position and/or posture 

15 caused by the deformation of the compliance mechanism 42 in a feedforward manner, thereby enabling to effect a 
walking which is closer to the desired gait when compared to a system without this deformation compensation control. 
[0155] Based on the above, again returning to the explanation of the flow chart of Figure 1 0, the aforesaid compen- 
sating angles are determined in S34. 

[0156] Figure 28 is a flow chart showing the subroutine of this determination. 

20 [0157] Explaining this with reference to the figure, the program begins in S100 in which the aforesaid vector V is 
calculated and proceeds to S102 in which the distribution-weight variables at the current time t are calculated based 
on the set characteristics shown in Figure 20. The program then proceeds to S104 in which the compensating total 
floor reaction force's moment Mdmd is distributed to, or divided into the feet compensating moment Mdmddbv and the 
foot compensating moment Mdmdnx(y) according to Eqs. 3 and 4, to S1 06 in which the feet compensating angle Gdbv 

25 is determined in the manner mentioned above. And, the program proceeds to S1 08 in which the foot compensating, 
angle 6nx(y) is determined. 

[0158] Returning to the flow chart of Figure 10, the program then proceeds to S36 in which the amount of deformation 
compensation is calculated based on the desired foot floor reaction force, to S38 in which the desired feet position 
and/or posture is corrected based on the compensating angles 0dbv, Gnx(y) and the amount of deformation compen- 

30 sation to determine the corrected feet position and/or posture with deformation compensation. 

[0159] The program then proceeds to S40 in which the joint displacement commands are calculated based on the 
body position and posture and the corrected desired feet position and/or posture with deformation compensation, to 
S42 in which the actual joint displacements are servo-controlled based on the calculated joint displacement commands, 
to S44 in which the time is updated by At, and returns to S14 to repeat these procedures. 

35 [01 60] With the configuration mentioned above, it can be summarized that almost no interference between the actual 
total floor reaction force control and the actual foot floor reaction force control occurs, thereby enabling to facilitate 
control of the actual total floor reaction force and the actual foot floor reaction force. 

[0161] To be specific, the composite compliance control has the following improvements over the technique proposed 
eariier. That is, in the ankle-compliance control proposed in 5(1 993) - 305584, the actual floor reaction force's moment 

JO acting on a fixed point at the foot such as the ankle or a reference sole point is detected and the foot is rotated about 
the point in response to the detected moment. In the composite compliance control system, the actual foot floor reaction 
force's moment acting on the moving desired foot floor reaction force central point is calculated, and the control is 
modified in such a manner that the foot is rotated about the desired foot floor reaction force central point in response 
to the calculated moment such that the moment about the point becomes a desirable value. 

45 [0162] As a result, it becomes possible to easily control the actual total floor reaction force and the actual foot floor 
reaction force, with almost no interference therebetween. In orderto further minimize the interference, it is alternatively 
possible to select the point of action at a more appropriate location within the floor-contact sole region at every instant. 
[01 63] Further, it becomes possible to control the floor reaction force acting on the robot , i.e., actual total floor reaction 
force's moment about the desired total floor reaction force central point (desired ZMP) and actual foot floor reaction 

50 force's moment about the desired foot floor reaction force central point, easily and appropriately. In other words, when 
compared to a case where the two kinds of controls are combined, no control conflict exists, and the actual total floor 
reaction force and the actual foot floor reaction force do not deviate from desired values or do not diverge. 
[0164] Accordingly, the system can appropriately control the floor reaction force acting on the robot, even when the 
robot walks on the floor having not only a slant or undulation extending over a relatively long distance, but also on an 

55 unexpected local slant or level difference, without being affected thereby. 

[0165] Further, the system can effect the posture stabilization control of the legged mobile robot easily and can 
decrease the impact that the robot receives at foot landing, thereby improving the ability of the robot to contact or touch 
the floor and preventing it from slipping or spinning, in addition, it can mitigate the load of the actuators of the robot. 
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[0166] In the former control disclosed in 5(1 993)-305586, the moment component of the actual total floor reaction 
force (resultant force of the foot floor reaction forces) about the desired total floor reaction force central point (desired 
ZMP) is detected and the control is conducted such that the detected value becomes a desired value. On the contrary, 
in the composite compliance control system, the moment (about the desired total floor reaction force central point 

5 (desired ZMP)) generated by the resultant force of the translational forces, except for the moment components, of the 
foot floor reaction force acting about the desired foot floor reaction force central point) is detected and the control is 
controlled such that Lhe detected value becomes a desired value. (However, the former control may instead be used.) 
[0167] Figure 29 is a view, similar to Figure 1 6, but showing a modification of the composite compliance control. 
[0168] In this modification, the compensating motion is simplified, but has the same effect as that mentioned in the 

"0 above. Specifically, as the method to correct the foot position to manipulate the force component of the floor reaction 
force of each foot, instead of the method illustrated in Figure 16, the foot is moved only in the vertical direction as 
illustrated in Figure 29. The vertical-moving amount Z1 of the 1st foot and that Z2 of the 2nd foot are calculated in 
accordance with the following equation: 

'5 

Z1 =- line PQ1's distance * Gdbv 

Z2 = line PQ2's distance * G dbv Eq. 1 8 

20 [0169] in the equation, the value obtained by Eq. 15 is used as the value 6 dbv. 

[0170] Based on the foregoing, the floor shape estimation system according to the embodiment of the invention will 

be explained. 

[0171] The configuration of the floor shape estimation system according to the embodiment is almost the same as 
that of the composite compliance control system illustrated in Figure 4, more specifically that illustrated in Figure 18. 

25 However, as shown in Figure30, the composite compliance operation determinator partly illustrated by its compensating 
total floor reaction force's moment distributor 114a is different from that disclosed in Figure 18, and the determinator 
is additionally provided with a floor shape estimator 130 and adders 132 (i.e., 132a, 132b) that add the floor shape 
estimated value (outputted from the floor shape estimator 130) to the respective compensating angles. 
[01 72] Before entering the explanation of the figure, the concepts and terms that the floor shape estimator 1 30 uses 

30 in the estimation will be defined as follows. 

[0173] As illustrated in Figure 31 , a floor supposed in the desired gait is called a "supposed floor (or supposed floor 
surface)". An actual floor on which the robot walks is called an "actual floor". A difference or deviation of the actual 
floor shape relative to the supposed floor shape is called a "floor shape difference". 

[0174] The desired n-th foot floor reaction force central point Qn in the composite compliance control is defined as 
35 a point set at the sole (contact surface with the floor surface) of the n-th foot. Consequently, the desired n-th foot floor 
reaction force central point Qn moves with the foot 22R(L). In line therewith, a point (on the supposed floor) which is 
supposed to be in contact with the desired n-th foot floor reaction force central point Qn is called a "supposed n-th floor 
contact point Dn". 

[0175] As will be understood from the definition, in a period of time during which the n-th foot is in contact with the 
40 floor, the desired n-th foot floor reaction force central point Qn and the supposed n-th floor contact point Dn are in the 
same coordinate, when viewed from the supporting leg's coordinate system. On the other hand, in a period during 
which the robot 1 actually walks, a point at which a point on the actual n-th foot's sole (corresponding to the desired 
foot floor reaction force central point Qn) is in contact with the actual floor is called as a "actual n-th floor contact point 
Dnact" 

45 [0176] Figure 31 shows an example of the relationship between these points. The figure is a side view of the robot 
1 showing that walks towards the normal direction (same as the direction of vector V in the composite compliance 
control) of the vertical plane which passes the desired 1st foot floor reaction force central point Q1 and the desired 
2nd foot floor reaction force central point Q2. 

[0177] The desired posture (desired gait's posture at this instant) of the robot 1 and the cross section of the supposed 
50 floor in the vertical plane are shown by thin lines, whereas the actual foot position and/or posture of the robot 1 and 
the cross section of the actual floor in the vertical plane are shown by thick lines. The actual n-th floor contact point at 
this situation is a point on the actual floor surface and its position is that shown in Figure 31 . 

[01 78] Further, as an index for expressing the floor shape difference quantitatively, foot-to-foot floor height difference 
error, foot-to-foot floor inclination difference and foot floor inclination difference are defined as follows. 
55 [01 79] Specifically, a difference between a height of the 2nd floor contact point and that of the 1 st floor contact point 
is called a "foot-to-foot floor height difference". This is calculated as follows. 
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Foot-to-foot floor height difference = 2nd floor contact point's height - 1st 

floor contact point's height Eq. 19 

5 

[0180] The error between an actual foot-to-foot floor height difference and a supposed foot-to-foot floor height dif- 
ference is called the "foot-to-foot floor height difference error". The inclination of a line whose initial point is the 1st 
floor contact point and whose terminal point is the 2nd floor contact point is called a "foot-to-foot floor inclination". This 
is calculated as follows: 

10 

Foot-to-foot floor inclination = atan ((2nd floor contact point's height - 1st 
floor contact point's height)/(horizontal distance between 2nd floor contact point and 1st 

15 

floor contact point)) Eq. 20 

[0181] In the above, "atan" in the equation indicates the inverse function of tangent in the trigonometric function. 
[0182] The difference between the actual foot-to-foot floor inclination and the supposed foot-to-foot floor inclination 
20 is called the "foot-to-foot floor inclination difference". Figure 31 shows this. In the illustrated situation, the foot-to-foot 
floor inclination difference is positive. The inclination of floor surface at the n-th floor contact point is called an "n-th 
foot floor inclination". 

[0183] The difference between the actual n-th foot floor inclination and the supposed n-th foot floor inclination is 
called the "n-th foot floor inclination difference". Figure 32 shows this. In Figure 32, the robot 1 is viewed from the back 
25 The desired posture (desired gait's posture at this instant) of the robot 1 and the cross section of the supposed floor 
in the vertical plane are shown by thin lines, and the actual foot position and/or posture of the robot 1 and the cross 
section of the actual floor in the vertical plane are shown by thick lines. The 1st foot floor inclination difference is 
negative in the illustrated example. 

[0184] The foot-to-foot floor inclination and the foot-to-foot floor inclination difference are expressed by angles rotated 
30 about the aforesaid vector's V-direction. The n-th foot floor inclination and the n-th foot floor inclination difference are 
expressed by angles rotated in the X and Y directions. 

[0185] The foot-to-foot floor inclination difference, the n-th foot floor inclination difference and the compensating 
angles in the composite compliance control have the following relationships thereamong. That is, the foot-to-foot floor 
inclination difference is, in the two-leg supporting period, corresponding to the feet compensating angle necessary for 

35 changing a line (connecting the desired 1st foot floor reaction force central point Q1 and the desired 2nd foot floor 
reaction force central point Q2 of the robot 1 that walks as required by the desired gait) from a situation where it extends 
in parallel with the desired floor surface, to a situation where it extends in parallel with the actual floor surface. 
[0186] Further, the n-th foot floor inclination difference is corresponding to the n-th foot compensating angle neces- 
sary for changing the n-th foot (of the robot that walks as required by the desired gait) from a situation where it is in 

40 parallel with the desired floor (floor surface) to a situation where it is in parallel with the actual floor surface. 

[0187] Accordingly, when the foot-to-foot floor inclination difference is estimated during walking and when the esti- 
mated value of the foot-to-foot floor inclination difference is added to the feet compensating angle, the actual total floor 
reaction force will be the same as that when the robot walks on the supposed floor, even if the foot-to-foot floor inclination 
difference exists. Moreover, when the n-th foot floor inclination difference is estimated during walking and when the 

45 estimated value of the n-th foot floor inclination difference is added to the n-th foot compensating angle, the actual n- 
th foot floor reaction force will be the same as that when the robot walks on the supposed floor, even if the n-th foot 
floor inclination difference exists. 

[0188] It should be noted that, when the modification of the composite compliance control where the feet are moved 
only in the vertical direction is used, instead of an estimated value of the foot-to-foot floor inclination difference, it 
50 suffices if an estimated value of the foot-to-foot floor height difference error is used. Since there is no substantial 
difference therebetween, the explanation will be made only for the case where the estimated value of the foot-to-foot 
floor inclination difference is used. 

[01 89] In the configuration illustrated in Figure 30, inputs to the floor shape estimator 1 30 will be classified as follows. 

55 1) various desired floor reaction force central points (i.e., the desired total floor reaction force central point and the 

desired n-th foot floor reaction force central point); 

2) supposed floor surface shapes (coordinates of the supposed n-th floor contact point and the supposed n-th foot 
floor inclination): 
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3) desired posture to which the actual robot should finally follow (corrected desired foot position and/or posture 
with deformation compensation), or the actual joint displacements or their average with a frequency weight; 

4) body inclination error; 

5) actual floor reaction force (force component and moment component of the actual n-th foot floor reaction force). 

5 

[0190] When the parameters illustrated in Figure 30 are really used as the inputs to the floor shape estimator 130, 
the estimation accuracy is maximized. However, other parameters may instead be used as discussed in a second 
embodiment of the invention. 

[0191] Figure 33 is a biock diagram showing the processing of the floor shape estimator 130. 
10 [0192] Explaining elements constituting the floor shape estimator 1 30 illustrated in Figure 33, it includes a mechanism 
compliance model 134. 

[0193] The mechanism compliance model 1 34 calculates the amount of deformation of the compliance mechanism 
42 when the input to the robot geometric model 110 (illustrated in Figure 4), i.e., the corrected desired foot position 
and/or posture with deformation compensation is subject to the actual floor reaction force, and determines estimated 

15 foot position and/or posture after mechanism deformation by adding the calculated amount of deformation to the cor- 
rected desired foot position and/or posture with deformation compensation. The estimated foot position and/or posture 
after mechanism deformation is expressed by a position and/or posture relative to the body 24. 
[0194] When the delay in the robot joint displacement control can not be neglected, as illustrated in Figure 34, it 
suffices if the corrected desired foot position and/or posture with deformation compensation is inputted to the mecha- 

20 nism compliance model 134 via a low-pass filter 136 having a characteristic corresponding to the delay. 

[01 95] Alternatively, as illustrated in Figure 35 , it suffices if the actual foot position and/or posture without deformation 
is calculated by a robot geometric model 137 (model similar to the model 110 illustrated in Figure 4) from the actual 
joint displacements and, instead of the corrected desired foot position and/or posture with deformation compensation, 
the calculated value is inputted to the compliance model 134. 

25 [0196] Alternatively, it suffices if a weighted average of the actual foot position and/or posture without deformation 
and the corrected desiredfoot position and/or posture with deformation compensation is calculated using the frequency 
weight and, instead of the corrected desired foot position and/or posture with deformation compensation , the weighted 
average is inputted to the mechanism compliance model 134. 

[0197] Strictly speaking, the actual foot position and/or posture without deformation compensation should be used. 
30 However, if the joint displacement control is effectively conducted, any of the method mentioned above can be used. 
[0198] Further, when the input to the robot geometric model 110 does not include the value with deformation com- 
pensation, it suffices if the corrected desiredfoot position and/or posture without deformation compensation is inputted 
to the robot geometric model 134. 

[0199] Returning to the explanation of Figure 33, the floor shape estimator 130 has a foot-to-foot floor inclination 
35 difference estimator 138. Specifically, in the figure, blocks for estimating the foot-to-foot floor inclination difference from 
the estimated foot position and/or posture after the mechanism has deformed, a coordinate of the desired foot floor 
reaction force central point relative to the desired foot, the supposed floor contact points and the body inclination error 
are called the "foot-to-foot floor inclination difference estimator". The processing of this estimator will be explained in 
the following. 

40 [0200] First, the estimator calculates a coordinate of the desired foot floor reaction force central point after mechanism 
deformation Qn" from the estimated foot position and/or posture after mechanism deformation and the coordinate of 
the desired foot floor reaction force central point Qn relative to the desired foot. The desired foot floor reaction force 
central point after mechanism deformation Qn" is expressed as a position relative to the actual body. 
[0201] More specifically, the desired n-th foot floor reaction force central point after mechanism deformation Qn" is 

45 calculated from the following equation, when the position of the desired n-th foot floor reaction force central point Qn 
relative to the foot is expressed by a vector Rn, the estimated n-th foot position after mechanism deformation is ex- 
pressed by a vector Un and the estimated n-th foot posture after mechanism deformation is expressed by a matrix An. 



5Q Qn" = AnRn + Un Eq. 21 

[0202] Next, the estimator calculates an inclination efdbv" about the vector V of a vector Q1"Q2" (whose initial point 
is Q1 " and terminal point is Q2") relative to the actual body. The vector V is the vector defined in the composite com- 
pliance control. 

55 [0203] And ; the estimator also calculates an inclination e fdbv about the vector V of a vector D1D2 (whose initial 
point is D1 and whose terminal point is D2) relative to the desired body, from the coordinate of the supposed n-th floor 
contact point Dn relative the desired body. 

[0204] Here, the supposed n-th floor contact point Dn is the same as the desired n-th foot floor reaction force central 



22 



BNSDOCID: <£P. 



1291136A1 _L> 



EP1 291 136 A1 



point Qn in a period during which the n-th foot is in contact with the floor, as defined above. At a time immediately 
before the n-th foot lands in the desired gait, the supposed n-th floor contact point Dn is set at the desired n-th foot 
floor reaction force central point Qn supposed at the foot landing. At a time immediately after the n-th foot is lifted in 
the desired gait the supposed n-th floor contact point Dn is set at the desired n-th foot floor reaction force central point 

5 Qn supposed at the foot lifting 

[0205] Next, the estimator calculates an inclination about the vector V of the vector Q1"Q2" (whose initial point is 
Q1" and whose terminal point is Q2") relative to the desired body by adding an V-direction component of the body 
inclination error e errv to the aforesaid inclination efdbv", and obtains a V-direction instantaneous foot-to-foot floor 
inclination difference calculation value 9 fdberrrv by subtracting e fdbv from the calculated inclination. 

10 [0206] Next, the estimator obtains 0 fdberrdiffv by subtracting an estimated foot-to-foot floor inclination difference 
(just obtained) G fdbcstmv from 9 fdberrrv. 

[0207] Then, in the estimator, a foot-to-foot floor inclination difference estimation interrupter 140 determines whether 
the foot-to-foot floor inclination difference estimation should be conducted or interrupted. 

[0208] The foot-to-foot floor inclination difference estimation interrupter 1 40 immediately outputs the aforesaid value 
15 Ofdberrdiffv inputted thereto when it determines that the foot-to-foot floor inclination difference estimation should be 
conducted (continued), whereas it outputs 0 when it determines that the estimation should be interrupted. The details 
of the foot-to-foot floor inclination estimation interrupter 140 will be explained later. 

[0209] Next, the estimator calculates the estimated foot-to-foot floor inclination difference 9 fdbestmv by integrating 
the output of the foot-to-foot floor inclination difference estimation interrupter 1 40 at an integrator 1 42 by an integration 
20 constant Kdbestm. The estimated foot-to-foot floor inclination difference Gfdbestmv is an estimated value of the foot- 
to-foot floor inclination difference illustrated in Figure 31 and is expressed as an angle about V. The above is processed 
by the foot-to-foot floor inclination difference estimator 138. 

[0210] As illustrated in Figure 30, the estimated foot-to-foot floor inclination difference 0 fdbestmv is added to the 
feet compensating angle 9 dbv, and the sum is inputted to the corrected desired foot position and/or posture calculator 

25 1 1 4g as a feet compensating angle with floor shape estimation edbv\ 

[021 1 ] By repeating the processing mentioned above, the estimated foot-to-foot floor inclination difference 9 fdbestmv 
converges to the actual foot-to-foot floor inclination difference. The foot position and/or posture is corrected by the 
estimated foot-to-foot floor inclination difference 9 fdbestmv such that the influence of the foot-to-foot floor inclination 
difference upon the actual floor reaction force is absorbed. 

30 [0212] In the above, when the feet are, in fact, on the floor, the aforesaid Qn" must be equal to the actual n-th floor 
contact point Dnact illustrated in Figure 31. Accordingly, a difference efdberrv between the inclination of the vector 
Q1"Q2" and that of the vector D1D2 must be equal to the foot-to-foot floor inclination difference illustrated in Figure 31 . 
[0213] However due to a vibration in mechanical or control system of the robot 1 and an electric noise, the value 9 
fdberrv (obtained from the calculation algorithm mentioned above), in fact, deviates or fluctuates markedly around the 

35 actual foot-to-foot floor inclination difference. For that reason, if the value 9fdberrv is immediately used as the estimated 
value of the foot-to-foot floor inclination difference and is added to the feet compensating angle as illustrated in Figure 
30, the foot 22R(L) of the robot 1 may oscillate or vibrate hard. 

[0214] However, if the foot-to-foot floor inclination difference estimation interrupter 140 (illustrated in Figure 33) de- 
termines all the time that the estimation should be conducted or continued, the transfer function of portions from 9fdberrv 

■40 to the estimated foot-to-foot floor inclination difference 9fdbestmv will be the same as the characteristic of a low-pass 
filter with first-order lag. In other words, the estimated foot-to-foot floor inclination difference Gfdbestmv will be the same 
as the value obtained by passing Gfdberrv through the low-pass filter. As a result, if the estimated foot-to-foot floor 
inclination difference 9 fdbestmv is added to the feet compensating angle as illustrated in Figure 30, the oscillation or 
vibration of the foot 22R(L) is unlikely to occur. 

45 [0215] Incidentally in the illustrated example, the time constant of the low-pass filter is 1/Kdbestm. Other example 
than that illustrated in Figure 33, the configuration can be modified provided that the transfer function of portions from 
Gfdberrv to the estimated foot-to-foot floor inclination difference Ofdbestmv becomes equal to the low-pass filter. With 
this, it becomes possible to prevent the foot 22R(L) from oscillating or vibrating. 

[0216] The characteristic features of the foot-to-foot floor inclination estimator 138 will then be explained. 

50 [0217] In order to generate the compensating moment's V-direction component Mdmddbv necessary for posture 
stabilization control, if the feet compensating angle 9dbv (illustrated in Figure 30) is changed to vary the corrected 
desired foot position and/or posture with deformation compensation (which is the finally desired foot position and/or 
posture), insofar as the feet are perfectly in contact with the floor, the actual position of the desired foot floor reaction 
force central point after mechanism deformation Qn" is at to the actual n-th floor contact point Dnact and remains 

55 unchanged. Rather, the compliance mechanism 42 deforms or the position and/or posture of the body 24 changes. In 
other words, the compliance mechanism 42 deforms or the position and/or posture of the body 24 changes such that 
the change of the corrected desired foot position and/or posture with deformation compensation is canceled. 
[0218] In the foot-to-foot floor inclination difference estimator 138 in this embodiment, since the amount of mechanism 
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deformation (calculated by the mechanism compliance model 134 based on the actual floor reaction force) and the 
actual body posture inclination error obtained from the output of the inclination sensor 36 are added at a point before 
the estimated foot-to-foot floor inclination difference e fdbestmv is calculated based on the corrected desired foot po- 
sition and/or posture with deformation compensation, even if the corrected desired foot position and/or posture with 
s deformation compensation fluctuates, the estimated foot-to-foot floor inclination difference 6 fdbestmv is free from the 
fluctuation. 

[021 9] This indicates that the posture control and the foot-to-foot floor inclination difference estimation are configured 
such that they do not conflict with each other. Accordingly, if the estimated foot-to-foot floor inclination difference 0 
fdbestmv is added to the feet compensating angle as illustrated in Figure 30, the margin of control stability (i.e., un- 
10 likeness of oscillation) is not degraded. This is an advantages over the aforesaid prior art mentioned on the Journal of 
the Robotics Society of Japan. 

[0220] Further, when the estimated foot-to-foot floor inclination difference 0 fdbestmv is added to the feet compen- 
sating angle as illustrated in Figure 30, since the influence of the foot-to-foot floor inclination difference, if it exists, can 
be canceled, the actual total floor reaction force will be the same as a case where the robot walks on the supposed 
15 floor in a steady-state manner. In addition, since the estimation of the foot-to-foot floor inclination difference and the 
correction of the compensating angle are conducted at every control cycle, even if the floor shape is being changed 
during the estimation, the system can surely estimate the foot-to-foot floor inclination difference afterchange and cancel 
the influence by the estimated value. 

[0221] It should be noted here that, since the foot-to-foot floor inclination estimator 138 includes the low-pass filter 
20 as mentioned above, if the floor shape has changed at the time of foot landing or thereafter, the actual total floor reaction 
force is temporarily affected by the foot-to-foot floor inclination difference. However, the influence will then be attenuated 
by the time constant 1/Kdbestm. 

[0222] As illustrated in Figure 33, the floor shape estimator 1 30 has a foot floor inclination difference estimator 1 44. 
Specifically, in the figure, blocks for estimating the 1st foot floor inclination difference's X-componentfrom the estimated 
25 1st foot position and/or posture after mechanism deformation, the supposed 1st foot floor inclination and the body 
inclination difference's X-component are called the foot floor inclination difference estimator", more specifically, a "1st 
foot floor inclination difference's X-component estimator". The processing of the estimator will be explained in the 
following. 

[0223] First, the estimator calculates an X-component of inclination of the estimated 1st foot position and/or posture 
30 after mechanism deformation (relative to the supposed 1st foot floor inclination) Gflerrx" based on the estimated 1st 
foot position and/or posture after mechanism deformation and the supposed 1 st foot floor inclination. This corresponds 
to a sum obtained by adding the bending angle (illustrated in Figure 2) estimated from the actual floor reaction force, 
to a difference between the desired 1st foot posture inclination and the supposed 1st foot floor inclination. Only a Y- 
component thereof is illustrated in Figure 2. 
35 [0224] Next, the estimator calculates an instantaneous 1 st foot floor inclination difference's X-component ef1 errx by 
adding the body inclination difference's X-direction component e errx to the aforesaid value ef 1 errx" 
[0225] Next, the estimator obtains 6f1 errdiffx by subtracting, from 9f 1 errx, an estimated 1 st foot floor inclination dif- 
ference Gf1 estmx (just calculated). 

[0226] Then, in the estimator, a foot floor inclination difference estimation interrupter 146, more precisely a 1st foot 
+o floor inclination difference's X-component estimation interrupter determines whether the 1st foot floor inclination dif- 
ference's X-component estimation should be conducted or interrupted. 

[0227] The 1 st foot floor inclination difference's X-component estimation interrupter immediately outputs the aforesaid 
value Gf1 errdiffx inputted thereto when it determines that the 1st foot floor inclination difference's X-component esti- 
mation should be conducted (continued), whilst it outputs 0 when it determines that the estimation should be interrupted. 
45 The details of the foot floor inclination estimation interrupter 146, more precisely the 1st foot floor inclination difference's 
X-component estimation interrupter will be explained later. 

[0228] Next, the estimator calculates the estimated 1 st foot floor inclination difference's X-component Of 1 estmx by 
integrating the output of the 1st foot floor inclination difference's X-component estimation interrupter at an integrator 
148 by an integration constant K1 estmx. The above is processed by the 1st foot floor inclination difference's X-corn- 
50 ponent estimator. 

[0229] As illustrated in Figure 30, the estimated 1st foot floor inclination difference's X-component Gf 1 estmx is added 
to the 1 st foot X-compensating angle 61 x, and the sum is inputted to the corrected desired foot position and/or posture 
calculator 1 1 4g as a 1 st foot X-compensating angle with floor shape estimation 01 x'. 

[0230] By repeating the processing mentioned above, the estimated 1 st foot floor inclination difference's X-compo- 
55 nent 0f1 estmx converges to the actual foot-to-foot floor inclination difference's X-component. The foot position and/or 
posture is corrected by the estimated foot-to-foot floor inclination difference 6f 1 estmx such that the influence of the 1 st 
foot floor inclination difference's X-component upon the actual floor reaction force is absorbed. 

[0231] It should be noted in the above that, although only the 1st foot floor inclination difference's X-component 
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estimator 144 is illustrated in Figure 33, the system includes a 1st foot floor inclination difference's Y-component esti- 
mator, a 2nd foot floor inclination difference's X-component estimator and a 2nd foot floor inclination difference's Y- 
component estimator and similar processing is conducted for estimating a 1 st foot floor inclination difference's Y-com- 
ponent a 2nd foot floor inclination difference's X-component and a 2nd foot floor inclination difference's Y-component. 
5 [0232] The discussion made on the foot-to-foot floor inclination difference estimation will also be applied to this foot 
floor inclination difference estimation. 

[0233] ThaL is, when the 1st foot is, in fact, in contact with the floor completely, more precisely when the 1st foot is 
actually in area-contact with the floor or when the 1 st foot is actually in line-contact with the floor at the Y-direction by 
the edge of the toe or heel of the sole, the calculated instantaneous 1 st foot floor inclination difference's X-component 

10 e f 1 errx must be equal to the actual 1 st foot floor inclination difference's X-component 

[0234] However, due to a vibration in mechanical or control system of the robot 1 and an electric noise, the value 
Bf1 errx (obtained from the calculation algorithm mentioned above), in fact, deviates or fluctuates markedly around the 
actual 1st foot floor inclination difference's X-component. For that reason, if the value Gflerrx is immediately used as 
the estimated value of the 1 st foot floor inclination difference's X-component and is added to the 1 st foot X-compensating 

15 angle as illustrated in Figure 30, the foot 22R(L) of the robot 1 may oscillate or vibrate hard. 

[0235] However, when the 1st foot floor inclination difference's X-component estimation interrupter (illustrated in 
Figure 33) determines all the time that the estimation should be conducted or contin ued, the transfer function of portions 
from Of 1 errx to the estimated 1st foot floor inclination difference's X-component G flestmx will be the same as the 
characteristic of a low-pass filter with first-order delay. In other words, the estimated 1st foot floor inclination difference's 

20 X-component G f 1 estmx will be the same as the value obtained by passing 0 f 1 errx through the low-pass filter. 

[0236] As a result, if the estimated 1st foot floor inclination difference's X-component Gf1 estmx is added to the 1st 
foot compensating angle as illustrated in Figure 30, the oscillation or vibration of the foot 22R(L) is unlikely to occur. 
Incidentally, in the illustrated example, the time constant of the low-pass filter is 1/K1 estmx. 

[0237] Other than that illustrated in Figure 33, the configuration can be modified provided that the transfer function 
25 of portions from Gflerrx to the estimated 1st foot floor inclination difference's X-component Gf 1 estmx becomes equal 
to the low-pass filter similar to the case of the foot-to-foot floor inclination difference estimation. With this, it becomes 
possible to prevent the foot 22R(L) from oscillating or vibrating. 

[0238] The characteristic features of the foot floor inclination difference estimator 144 will then be explained. 
[0239] Similar to the foot-to-foot inclination difference estimation, the following can be stated as the features. 

30 [0240] In order to generate the 1st foot compensating moment's V-component Mdmdl x necessary for posture sta- 
bilization control, if the 1st foot compensating angle 61 x (illustrated in Figure 30) is varied to change the corrected 
desired foot position and/or posture with deformation compensation (which is the finally desired foot position and/or 
posture), insofar as the 1st foot is perfectly in contact with the floor, the actual foot posture remains unchanged. Rather, 
the compliance mechanism 42 deforms or the position and/or posture of the body 24 changes. In other words, like the 

35 case of the foot-to-foot floor inclination difference estimation, the compliance mechanism 42 deforms or the position 
and/or posture of the body 24 changes such that the change of the corrected desired foot position and/or posture with 
deformation compensation is canceled. 

[0241] In the 1 st foot floor inclination difference's X-component estimator in this embodiment, the amount of mech- 
anism deformation (calculated by the mechanism compliance model 134 based on the actual floor reaction force) and 

■40 the actual body posture inclination error obtained from the output of the inclination sensor 36 are added at a point 
before the estimated 1st foot floor inclination difference's X-component 9f1 estmx is calculated based on the corrected 
desired foot position and/or posture with deformation compensation. With this, even if the corrected desired foot position 
and/or posture with deformation compensation fluctuates, the estimated 1st foot floor inclination difference's X-com- 
ponent 6 f1 estmx is free from the fluctuation. 

■*5 [0242] This indicates that the posture control and the 1st foot floor inclination difference estimation are configured 
such that they do not conflict with each other. Accordingly, if the estimated 1st foot floor inclination difference's X- 
component 6f1 estmx is added to the 1st foot compensating angle as illustrated in Figure 30, the margin of control 
stability is not degraded, like the case of the foot-to-foot floor inclination difference estimation. 

[0243] Further, when the estimated 1st foot floor inclination difference's X-component Gf1 estmx is added to the 1 st 
50 foot compensating angle as illustrated in Figure 30, since the influence of the 1st foot floor inclination difference, if it 
exists, can be canceled, the actual 1st foot floor reaction force will be the same as a case where the robot walks on 
the supposed floor in a steady-state manner. 

[0244] In addition, since the estimation of the 1 st floor inclination difference and the correction of the compensating 
angle are conducted at every control cycle, even if the floor shape is being changed during the estimation, the system 
55 can surely estimate the 1 st foot floor inclination difference and cancel the influence by the estimated value. 

[0245] It should be noted here that, since the 1st foot floor inclination difference estimator includes the low-pass filter 
as mentioned above, if the floor shape has changed at the time of the 1st foot landing or thereafter, the actual 1st foot 
floor reaction force is temporarily affected by the 1st foot floor inclination difference, but the influence will then be 
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attenuated by the time constant 1/K1estmx. 

[0246] The characteristic features of the floor shape estimation in this embodiment will next be summarized. 
[0247] The composite compliance control on which the floor shape estimation of this embodiment is based, is char- 
acterized in that the controls of the actual total floor reaction force and foot reaction force through the manipulation of 

s the corresponding one of the compensating angles are configured not to be in conflict with each other. 

[0248] Therefore, when the operations to determine the estimated foot-to-foot floor inclination difference 6 fdbestmv, 
to add it to the feet compensating angle Bdbv, to determine estimated foot inclination differences 8f1 estmx, 6f1estmy, 
Gf2estmx and 0f2estmy, and to add each to the respective foot compensating angles 01 x, 81y, 82x and 62y are con- 
ducted at the same time, the operations do not conflict with each other and no oscillation happens. 

10 [0249] This characteristic feature as well as the characteristic features of the foot-to-foot floor inclination difference 
estimator 138 and the foot floor inclination difference estimator 144 can yield the following advantages. 

1 ) It becomes possible to conduct the following controls or operations at the same time, i.e. , to conduct the posture 
stabilization control to manipulate the feet compensating angle 6dbv and the respective foot compensating angles 

15 91 x, 81 y, B2x and B2y to control the actual floor reaction force, to estimate the foot-to-foot floor inclination difference, 

to add it to the feet compensating angle Bdbv, to estimate the foot floor inclination differences and add them to the 
respective foot compensating angles. 

Briefly speaking, it becomes possible to estimate a plurality of parameters indicative of the floor shape differ- 
ences at the same time, while conducting the respective compensation operations at the same time to cancel the 

20 influence of the estimated floor shape differences on the floor reaction force. In addition, it becomes possible to 

conduct the feet compensation operation for the posture stabilization. 

2) The controls are unlikely to oscillate. (This is because no approximation to limit to a fine angle is not conducted 
in the composite compliance control.) 

3) The estimation accuracy of the floor shape difference is high. 

25 4) Since oscillation due to the conflict with the posture stabilization control is unlikely to occur, in the estimators 

138, 144, it becomes possibleto increase the gains (Kdbestm, Klestmx, Klestmy, K2estmx, K2estmy) and hence, 
it becomes possible to decrease or shorten the estimation time constant. With this, since it becomes possible to 
enhance the response in the feet compensation for canceling the influence of the floor shape difference upon the 
floor reaction force, the influence can be eliminated in a short period of time. 

so 5) Since the estimation of the floor shape difference and the foot compensation for canceling the influence of the 

floor shape difference on the floor reaction force are conducted all the time, even if the floor shape changes during 
the operation , the influence remains temporarily and attenuates with respect to time by the estimation time constant. 

[0250] Then, the aforesaid estimation interrupters 140, 146 will be explained. 
35 [0251] First, the operation of the 1st foot floor inclination difference's Y-component estimation interrupter. 

[0252] The 1 st foot floor inclination difference's Y-component estimation interrupter determines whether the estima- 
tion of the 1st foot floor inclination difference's Y-component can be conducted normally and when it determines it 
impossible to conduct the estimation normally, it interrupts or discontinues the estimation. 

[0253] More specifically, basically, the interrupter outputs 0 (i.e., to input 0 in the integration input to hold the integral 
40 value) to interrupt the estimation in a situation where the accuracy of the estimation of the 1st foot floor inclination 
difference's Y-component is liable to degrade or in a situation where the estimation of the 1st foot floor inclination 
difference's Y-component is liable to oscillate. In a situation other than the above, the interrupter outputs the inputted 
estimated 1st foot floor inclination difference's Y-component immediately to conduct or continue the estimation. 
[0254] If the 1 st foot floor inclination difference's Y-component estimator does not operate as desired and continues 
45 to pass the input immediately, the estimated 1 st floor inclination difference's Y-component may oscillate under a certain 
situation. 

[0255] This situation where the estimated 1 st foot floor inclination difference's Y-component oscillate in this embod- 
iment will then be explained. 

[0256] It is supposed a situation, illustrated in Figure 36, where, since the robot 1 tilted forward in the leg-supporting 
50 period J the 1st foot's Y-compensating angle was manipulated in the posture stabilization control so as to generate a 
posture stabilization force markedly. However, this resulted in lifting of the 1st foot heel (which had to be in contact 
with the floor). It is supposed that the actual floor shape was equal to the supposed floor shape, 
[0257] In this situation, the angle 6f1 erry (obtained by adding the body inclination difference's Y-component Berry to 
the estimated 1 st foot posture inclination Y-component after mechanism deformation Bf1 erry" relative to the supposed 
55 ist foot floor inclination's Y-component) becomes a positive value, as illustrated in Figure 36 

[0258] When the 1st foot floor inclination difference's Y-component estimation interrupter outputs or pass the inputted 
Bflerrdiffy immediately, as mentioned above, the transfer function from 0f1erry to the estimated foot-to-foot floor incli- 
nation difference 6f1 estmy is a first-order delay value. In other words, Of 1 estmy follows Bf1 erry with a small lag. Oflestmy 
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is added to the 1st foot compensating angle 81y to produce 91y'. Only the configuration of 6f1x' is illustrated in Figure 
30, that of 61 y is similar. 

[0259] Then , this 1 st foot Y-compensating angle 61 y* is added to the desired 1 st foot position and/or posture and the 
amount of deformation calculated from the desired floor reaction force (floor reaction force in the desired gait) is sub- 

5 tracted therefrom (i.e., an amount of compensating deformation which cancels the amount of deformation is added), 
and the corrected desired 1st foot position and/or posture with deformation compensation is calculated. 
[0260] In addition, an amount of mechanism deformation is calculated by the mechanism compliance model 134 
based on the actual floor reaction force, and is added to the corrected desired 1st foot position and/or posture with 
deformation compensation to produce the estimated 1st foot position and/or posture after mechanism deformation. By 

w subtracting the supposed 1 stfoot inclination's Y-componentfrom the inclination's Y-component and by adding the body 
inclination difference's Y-component 6 erry thereto, the aforesaid value 6f1erry is updated. 

[0261] From these relationships mentioned above, the relationship between 6f1erry and eflestmy can be expressed 
as follows. In the following, the 1st foot mechanism deformation compensating angle due to desired floor reaction force 
indicates the amount of compensation for canceling the amount of deformation of the compliance mechanism 42 men- 
15 tioned above. 

6f1erry = Oflestmy -+- 61 y + 1st foot mechanism deformation 
^ compensating angle's Y-component due to desired floor reaction force + eerry + 1st foot 

mechanism deformation angle's Y-component calculated from actual floor reaction force + 
desired 1st foot position and/or posture inclination's Y-component - supposed 1st foot 

inclination's Y-component Eq. 22 

[0262] During the one-leg supporting period in the desired gait, (desired 1 st foot position and/or posture inclination's 
Y - component - supposed 1st foot inclination's Y-component) is 0. Accordingly, Equation 22 Is rewritten as follows. 

30 

6f 1 erry = 6f 1 estmy + 6 1 y + 1 st foot mechanism deformation compensating 
angle's Y-component due to desired floor reaction force + eerry + 1 st foot mechanism 

35 

deformation angle's Y-component calculated from actual floor reaction force Eq. 23 

[0263] When the 1st foot is in contact with the floor completely, whatever (6 f1 estmy + 61 y + 1st foot mechanism 
deformation compensating angle's Y-component due to desired floor reaction force + 6erry) is, the right term of Equation 
40 23 becomes 0, since the mechanism deformation angle's Y-component calculated from the actual floor reaction force 
cancels it. As a result, 6f1 erry becomes always 0. Since 6f 1 estmy follows 6f 1 erry with first-order lag, it gradually con- 
verges to 0. This indicates that the estimator can accurately estimate the situation that the actual floor shape is equal 
to the supposed floor shape 

[0264] However, in the example illustrated in Figure 36, although the value (6f1 estmy + 61 y + 1st foot mechanism 
45 deformation compensating angle's Y-component due to desired floor reaction force + Gerry) is a large positive value, 
the actual floor reaction force does not grow sufficiently since the foot heel is out of contact with the floor. As a result, 
the mechanism deformation amount's Y-component can not be a negative value enough for canceling the aforesaid 
value. In other words, the right term of the equation does not become 0. In this case, 6 f 1 erry does not become 0 and 
hence, 6 f1 estmy does not converge to 0. This indicates that the estimated value was erroneous. 
50 [0265] In such a case, in particular in a case where a situation which satisfies Eq. 24 mentioned below continues, 
even if eflestmy is increased so as to be close to 6f1erry, since 6 flerry keeps the relationship of 6 flerry > efestmy, 
led from Equations 23 and 24, 6f1 erry becomes away form G 1 estmy. Repeating this, Gf 1 estmy oscillates in the positive 
direction more and more. This indicates that the estimated value is being oscillated. 

55 

e 1 y + 1 st foot mechanism deformation compensating angle's Y-component due 
to desired floor reaction force 4- 6 erry + 1st foot mechanism deformation angle's Y- 
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component calculated from actual floor reaction force > 0 Eq. 24 

[0266] The processing of the 1 st foot floor inclination difference's Y-component estimation interrupter in this embod- 
iment will then be explained with reference to Figure 37. The processing is repeated at every control cycle. 
[0267] In S30O, the estimation mode of 1 st foot floor inclination difference's Y-component is determined in response 
to the timing of the desired gait, 

[0268] Specifically, the estimation mode of the 1st foot floor inclination difference's Y-component can be one of a 
resetting mode, a holding mode and a preparation-completed mode. As illustrated in Figure 38, one among from the 
three is selected with respect to time in the desired gait. The illustrated transit pattern of mode is the gait illustrated in 
Figure 5. When the desired gait is changed ( the transit pattern of mode should be changed in response thereto. 
[0269] Then, in S302, the following processing is conducted in response to the estimation mode of the 1st foot floor 
inclination Y-component mode. 

[0270] Specifically, when the estimation mode of the 1 st foot floor inclination's Y-component is the resetting mode, 
the program proceeds to S304 in which the estimated 1st foot floor inclination difference's Y-component eflestmy is 
forced to return or converge to 0 gradually, and to S306 in which the output of the 1 st foot floor inclination difference's 
Y-component estimator interrupter is determined to be 0. 

[0271] As illustrated in Figure 38, the resetting mode is determined to be a period of time during which the 1st foot 
is completely out of the floor such that movement of the 1st foot does not influence the floor reaction force. When there 
is no relationship between the foot floor inclination difference of the 2nd foot floor contact point landed one step earlier 
and that of the 1st foot floor contact point to be landed next, it is preferably to return the estimated 1st foot floor inclination 
difference's Y-component to 0 forcibly and gradually as mentioned above. 

[0272] However, for example, if it has been known that undulation of the actual floor (which is not taken into account 
in the supposed floor) is calm, the existence of the relationship can be presumed to a fair extent, this may be modified 
such that the estimated 1st foot floor inclination difference's Y-component eflestmy converges gradually to a value 
obtained by multiplying a positive constant (which is smaller than 1 ) by the estimated 2nd foot floor inclination difference 
of the 2nd foot floor contact point landed one step earlier. This can further be modified such that a value to which the 
estimated value converges is determined, not from the estimated foot floor inclination difference's Y-component one 
step eariier, but from that several steps earlier, or is determined from both the estimated values one step earlier and 
several steps earlier. 

[0273] When the estimation mode of the 1st foot floor inclination's Y-component is the holding mode, the program 
proceeds to S308 in which the output of the 1 st foot floor inclination difference's Y-component interrupter is determined 
to be 0. Accordingly, the estimated 1st foot floor inclination difference's Y-component ef 1 estmy is held to a value at a 
time just before the mode is changed to the holding mode. 

[0274] The holding mode is determined to be a period of time during which the estimated 1st foot floor inclination 

difference's Y-component is liable to oscillate or the accuracy of estimation accuracy is liable to degrade. For example, 

as illustrated in Figure 38, it should be set within a period of time which starts when the heel or toe of the 1 st foot begins 

to leave the floor and continues for a fair amount of time after the 1st foot has left the floor completely. 

[0275] In a period of time other than the above, in other words in a period of time in which the estimation of the 1 st 

foot floor inclination difference's Y-component can be conducted normally, the estimation mode of the 1st foot floor 

inclination's Y-component is determined to be the preparation-completed mode. 

[0276] In this preparation-completed mode, the following processing will be conducted. 

[0277] Specification, the program proceeds to S31 0 in the flow chart in which the actual 1st foot floor reaction force 
central point (point's position) Q1act is determined based on the actual 1st foot floor reaction force's moment M1act 
and the actual 1st foot floor reaction force's force component F1act. The actual 1st foot floor reaction force central 
point (point' position) Q1act indicates a point of action on the foot sole about which moment components other than 
vertical component of the actual 1st foot floor reaction force's moment becomes 0. The X-coordinate of the actual 1 st 
foot floor reaction force central point Q1act is named a "Qlactx". 

[0278] The program then proceeds to S312 in which the actual 1st foot floor reaction force's Z-component (vertical 
component) Flactz and the aforesaid coordinate point Qlactx relative to preset 1st foot estimation permissible mini- 
mum floor reaction force F1min, 1st foot estimation permissible maximum area Qlmaxx and 1st foot estimation per- 
missible minimum area Qlminx satisfy at least one of the following conditions: 

Condition 1) Flactz < F1min 

Condition 2) Q1 actx > Q1 max and G flerrd iffy > 0 

Condition 3) Q1 actx < Q1 minx and Gf1 errdiffy < 0 
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[0279] As regards Condition 1 , the 1st foot estimation permissible minimum floor reaction force F1min is a threshold 
value indicating that the estimation of the 1 st foot floor inclination is impossible if the floor reaction force acting on the 
1 st floor is less than it, and should be preset to a value smaller than the weight of the robot 1 . In other words, Condition 
1 indicates a situation where accurate estimation of the 1 st foot floor inclination's Y-component is impossible since the 

5 pressure between the 1st foot and the floor is too small. 

[0280] As regards Conditions 2) and 3), when defining the X-coordinate of the toe as "xtoe" and the heel "xheel" as 
in the foot coordinate system, the 1 st foot estimation permissible maximum area Q 1 maxx should be preset to a value 
slightly smaller than xtoe, whilst the 1 st foot estimation permissible minimum area Q1 minx should be preset to a value 
slightly larger than xheel. Figure 39 illustrates the relationship therebetween. 

10 [0281] When the estimation is conducted in an ideal manner, the 1st foot estimated permissible maximum area 
Q1 maxx and the 1 st foot estimation permissible minimum area Q1 minx can be preset to values corresponding to the 
foot edge. However, in fact, there occurs a detection error which makes the estimation value to oscillate. In order to 
prevent this from occurring, as illustrated in Figure 39, the 1st foot estimation permissible maximum area Qlmaxx and 
the 1st foot estimation permissible minimum area Qlminx should be preset to points that reside inner than the edges 

15 by a fair amount. 

[0282] Condition 2) indicates a situation in which the integrator input 6 f1 errdiffy Is a positive value although the feet 
is about to lift, in other words, the estimated 1st foot floor inclination is being changed to a direction in which the foot 
heel lifts. When the estimation is continued in such a situation, the 1st foot Y-compensating angle with floor shape 
estimation will change to that direction more and more. 
20 [0283] Condition 3) indicates a situation in which the integrator input Gf 1 errdiffy is a negative value although the foot 
toe is about to lift, in other words, the estimated 1st foot floor inclination is being changed to a direction in which the 
foot toe lifts. When the estimation is continued in such a situation, the 1 st foot Y-compensating angle with floor shape 
estimation will change to that direction more and more. 

[0284] Returning to the explanation of the flow chart of Figure 37, when the result in S312 is affirmative and it is 
25 determined that at least one of Conditions 1) ; 2) and 3) is satisfied, since the estimation should be interrupted or 
discontinued, the program proceeds to S31 4 in which the output of the 1 st foot floor inclination difference's Y-component 
estimation interrupter is determined to be 0. On the other hand, when the result is negative in the step, since this 
indicates the there is no problem in conducting the estimation, the program proceeds to S316 in which the output is 
determined to be Of 1 errdiffy. 

30 [0285] It is sufficient if the equations of Conditions 1), 2) and 3) can indicate the situations mentioned above. Ac- 
cordingly, in Condition 1 ), instead of the actual 1 st foot floor reaction force's Z-component (vertical component) F1 actz, 
the component normal to the foot (named a "F1 actz 1 ") of the actual 1 st foot floor reaction force's force component F1 act 
can be used. 

[0286] In Conditions 2) and 3), instead of the actual 1st foot floor reaction force central point's position Q1 act, a point 
35 of action on the foot sole about which moment components other than that normal to the foot of the actual 1 st foot floor 
reaction force's moment becomes 0 can be used. This point is named a "Qlact"'. 

[0287] Then , explaining the processing of the 1 st foot floor inclination difference's X-component estimation interrupt- 
er, it conducts the processing similar to that of the 1st foot floor inclination difference's Y-component estimation inter- 
rupter. 

40 [0288] The differences therebetween are that, in Conditions 2) and 3), instead of those indicating lifting of foot toe 
or heel, those indicating lifting of foot's inner or outer edge are used. To be more specific, Conditions 2'and3 l ) mentioned 
below are newly used. In these conditions, as illustrated in Figure 39, Q1 maxy and Q1 rniny are preset to areas which 
are slightly towards the foot sole center from the edges. 

45 Condition 2') Q1 acty > Q1 maxy and Aflerrdiffx < 0 

Condition 3') Q1 acty < Q1 miny and Gf 1 errdiffx > 0 

[0289] Due to the direction of coordinate system, the sign of inequality are partially changed in the equations. In 
50 addition, as regards the transit timing of the estimation mode of the 1st foot floor inclination difference's X-component, 
the termination of the preparation-completed mode may be delayed slightly than that of the Y-component. This is 
because the 1 st foot floor inclination difference's X-component can be estimated insofar as the foot toe edge in contact 
with the floor 

[0290] It should also be noted that the processing or operation of the 2nd foot floor inclination difference estimation 
55 interrupter is the same as that of the 1st foot floor inclination difference estimation interrupter. 

[0291 ] The processing of the foot-to-foot floor inclination difference's estimation interrupter 1 40 will then be explained 
with reference to Figure 40. The processing is repeated at every control cycle. The processing is basically the same 
as that of the 1 st foot floor inclination difference's Y-component estimator, but is different from that in detailed portions. 
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[0292] In S400, the estimation mode of the foot-to-foot floor inclination is determined in response to the timing of the 
desired gait. 

[0293] Specifically, the estimation mode of the foot-to-foot floor inclination should similarly be determined in response 
to time in the desired gait, as illustrated in Figure 41 . The illustrated transit pattern of mode is also the gait illustrated 
5 in Figure 5. When the desired gait is changed, the transit pattern of mode should be changed in response thereto. 

[0294] Then, in S402, the following processing is conducted in response to the estimation mode of the foot-to-foot 

floor inclination. 

[0295] Specifically, when the estimation mode of the foot-to-foot floor inclination is a resetting mode, the program 
proceeds to S404 in which the estimated fooMo-foot floor inclination difference 0 fdbestmv is forced to return or con- 
io verge to 0 gradually, and to S406 in which the output of the foot-to-foot floor inclination difference estimation interrupter 
is determined to be 0. As illustrated in Figure 41 , the resetting mode is determined to be the one-leg supporting period 
such that movement of the feet compensating angle with floor shape estimation does not influence the floor reaction 
force. 

[0296] When there is no relationship between the foot-to-foot floor inclination difference at a time landed one step 
"5 earlier and that at a time to be landed next, it is preferably to return the estimated foot-to-foot floor inclination difference 
efdbestmv to 0 forcibly and gradually as mentioned above. However, for example, if it has been known that undulation 
of the actual floor (which is not taken into account in the supposed floor) is calm, the existence of the relationship can 
be presumed to a fair extent, this may be modified such that the estimated foot-to-foot floor inclination Gfdbestmv 
converges gradually to a value obtained by multiplying a positive constant (which is smaller than 1) by the estimated 
20 foot-to-foot floor inclination difference at the time landed one step earlier. 

[0297] This can further be modified such that a value to which the estimated value converges is determined; not from 
the estimated foot-to-foot floor inclination at the time one step earlier, but from that several steps earlier, or is determined 
from both the estimated values one step earlier and several steps earlier. 

[0298] When the estimation mode of the fooHo foot floor inclination is a holding mode, the program proceeds to 
25 S408 in which the output of the foot-to-foot floor inclination difference estimation interrupter is determined to be 0. 
Accordingly, the estimated foot-to-foot floor inclination difference Gfdbestmv is held to a value at a time just before the 
mode is changed to the holding mode. 

[0299] The holding mode is determined to be a period of time during which the estimated foot-to-foot floor inclination 
difference is liable to oscillate or the accuracy of estimation is liable to degrade. For example, as illustrated in Figure 
so 41 , it should be set within a period of time which starts at the termination of the two-leg supporting period and continues 
for a fair amount of time thereafter. 

[0300] In a period of time other than the above, in other words in a period of time in which the estimation of the foot- 
to-foot floor inclination difference can be conducted normally, the estimation mode of the foot-to-foot floor inclination 
difference is determined to be a preparation-completed mode. 
35 [0301] In this preparation-completed mode, the following processing will be conducted. 

[0302] Specification, the program proceeds to S410 in which the actual foot floor reaction force relative to preset 
foot-to-foot estimation permissible minimum floor reaction force Fmin and foot-to-foot floor inclination estimation per- 
missible ratio rmin satisfy at least one of the following conditions: 

4 o Condition 4) Flactz < Fmin or F2actz < Fmin 

Condition 5) F2actz/ (Flactz + F2actz) < rmin; and 

9 dberrdiffv > 0 
Condition 6) F2actz/ (Flactz + F2actz) < rmin; and 

8 dberrdiffv < 0 

45 

[0303] As regards Condition 4, the foot-to-foot floor inclination estimation permissible minimum floor reaction force 
Fmin is a threshold value indicating that the estimation of the foot-to-foot floor inclination is impossible if the floor 
reaction force acting on the 1st foot and 2nd foot is less than it, and should be preset to a value smaller than the weight 
of the robot 1. In other words, Condition 4 indicates a situation where accurate estimation of the foot-to-foot floor 
inclination is impossible since the pressure between the 1st and 2nd feet and the floor is too small. 
[0304] As regards Conditions 4) and 5), when the estimation is conducted in an ideal manner, the foot-to-foot floor 
inclination estimation permissible ratio rmin may be 0. However, in fact, there occurs a detection error which makes 
tie estimation value to oscillate. In order to prevent this from occurring, the foot-to-foot floor inclination estimation 
permissible ratio rmin should be preset to a positive value which is smaller than 1 by a fair amount. 
55 [0305] Condition 5) indicates a situation in which the integrator input efdberrdiffv is a positive value although the 2nd 
foot is about to lift, in other words, the estimated foot-to-foot floor inclination is being changed to a direction in which 
the 2nd foot lifts. When the estimation is continued in such a situation, the feet compensating angle with floor shape 
estimation will change to that direction more and more. 
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[0306] Condition 6) indicates a situation in which the integrator input 6 fdberrdiffv is a negative value although the 
1st foot is about to lift, in other words, the estimated foot-to-foot floor inclination is being changed to a direction in which 
the 1st foot lifts. When the estimation is continued in such a situation, the feet compensating angle with floor shape 
estimation will change to that direction more and more. 

5 [0307] Returni ng to the explanation of the flow chart of Figure 40, when it is determined that at least one of Conditions 
4), 5) and 6) is satisfied, since the estimation should be interrupted or discontinued, the program proceeds to S412 in 
which the output of the foot-to-fooi floor inclination difference estimation interrupter is determined to be 0. On the other 
hand, when the result is negative in the step, since this indicates the there is no problem in conducting the estimation, 
the program proceeds to S414 in which the output is determined to be 6 fdberrdiffv. 

io [0308] It is sufficient if the equations of Conditions 4), 5) and 6) can indicates the situations mentioned above. Ac- 
cordingly, instead of the actual n-th foot floor reaction force's Z-component (vertical component) Fnactz, the component 
normal to the foot (named a "Fnactz"') of the actual n-th foot floor reaction force's force component Fnact can be used. 
[0309] Having been configured in the foregoing manner the system can estimate the floor shape (that has been 
difficult in the prior art), i.e, the system can accurately estimate the inclination of the surface with which the foot is in 

:5 contact and the height difference between the surfaces with which the feet are in contact, at the same time, i.e., in a 
composite manner. 

[0310] Further, even if the floor shape is different from that supposed, the system can generate the floor reaction 
force as desired by correcting the foot trajectory to absorb the influence. 

[031 1 ] Further the system can eliminate the steady-state error between the actual floor reaction force and a desired 
20 value in the control as least as possible, which the prior art composite control could not achieve. In other words, the 
system can solve the steady-state error due to a deviation in the floor reaction force. 

[0312] Further, since the system is configured to interrupt or discontinue the floor shape estimation in a situation 
where the estimation accuracy is liable to degrade or the estimated value is likely to oscillate, in addition to the advan- 
tages mentioned above, the estimated value obtained will not be inappropriate. 
25 [031 3] It should be noted in the above, it is sufficient for the floor shape estimation system for a legged mobile robot 
according to this embodiment, if it estimates the floor shape in the manner mentioned above. In other words, it is not 
essential for the system to control the desired position posture such as the foot trajectory by manipulated the floor 
reaction force based on the estimated value. 

[0314] Next, a floor shape estimation system of a legged mobile robot according to a second embodiment of the 

30 present invention will be explained. 

[031 5] The second embodiment is a simplification of the first embodiment. Figure 42 illustrates the entire configuration 
of the floor shape estimation system according to the second embodiment. The configuration of the second embodiment 
is different in the inputs to floor shape estimator and the processing in the floor shape estimator. The rest of the con- 
figuration is the same as that of the first embodiment. 

35 [0316] Briefing the second embodiment, the compliance model 134 of the first embodiment is approximated by the 
configuration mentioned above and the block diagram illustrating the configuration is rewritten to be equivalent to that 
of the first embodiment. 

[031 7] Before entering the explanation of the floor shape estimator according to the second embodiment, terms and 
the compliance model used in the second embodiment will be explained. 

[0318] It is assumed here that, as illustrated at the top of Figure 43, the joint displacement control of the robot 1 is 
conducted in such a way that the relative relationship between the corrected desired foot position and/or posture with 
deformation compensation and the desired body position and/or posture are satisfied, then, as illustrated at the medium 
of the figure, the body 24 of the robot 1 is inclined or tilted by the body inclination error 0 err from the desired body 
posture and is supported in space, and then, as illustrated at the bottom of the figure, external force corresponding the 
^5 desired footfloor reaction force is imparted to each foot. Here, the inclination of a vector (about the aforesaid V-direction) 
whose initial point is a desired 1st footfloor reaction force central point Q1"' and whose terminal point is a desired 2nd 
footfloor reaction force central point Q2'" in the foot of this assumed situation, is called a "foot-to-foot inclination during 
ideal deformation". 

[0319] In addition, the posture inclination of the n-th foot of this assumed situation is called an "n-th foot inclination 
50 during ideal deformation". If the body inclination error, the feet compensating angle, the foot compensating angle, the 
estimated foot-to-foot floor inclination difference, the estimated 1st foot floor inclination difference and the estimated 
2nd foot floor inclination difference are all 0, the foot-to-foot inclination during ideal deformation becomes equal to the 
supposed foot-to-foot floor inclination, whilst the n-th foot inclination during ideal deformation becomes equal to the 
supposed n-th footfloor inclination. 
55 [0320] The sum of the supposed foot-to-foot floor inclination and the estimated foot-to-foot floor inclination difference 
is called an "estimated foot-to-foot floor inclination". This is expressed by an angle about the V-direction. The sum of 
the supposed n-th foot floor inclination and the estimated n-th foot floor inclination difference is called an "estimated 
n-th floor inclination" The difference between the foot-to-foot inclination difference during ideal deformation and the 
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estimated foot-to-foot floor inclination difference is called an "estimated feet interference angle edbintv" This is also 
expressed by an angle about the V-direction. 

[0321 ] The difference between the 1 st foot inclination during ideal deformation and the estimated 1 st foot floor incli- 
nation is called an "estimated 1 st foot interference angle 91 int". Similarly, the difference between the 2nd foot inclination 
5 during ideal deformation and the estimated 2nd foot floor inclination is called an "estimated 2nd foot interference angle 
62int". 

[0322] If it is assumed that the estimated foot-to-foot floor inclination and the actual foot-to-foot floor inclination are 
equal to each other and that the desired foot floor reaction force central point of the actual robot foot is in contact with 
the floor, the compliance mechanism 42 of the actual robot must deform so as to cancel the estimated feet interference 
to angle edbintv. 

[0323] This mechanism deformation must yield the V-direction component of the sum of the moments of the foot 
floor reaction force's force components of the two feet about the desired total floor reaction force central point. This V- 
direction moment component is called a "Mdestmv". 

[0324] Then, the relationship between the estimated feet interference angle G dblntv and the V-direction moment 
15 component Mdbestmv should be determined beforehand and stored in memory. Among of characteristics of the com- 
pliance mechanism 42 of the robot 1 , this relationship indicates only a characteristic of the feet interference angle and 
hence is called a "foot-to-foot mechanism compiiance model". This is illustrated in Figure 44 with reference numeral 
134a. 

[0325] If it is assumed that the estimated n-th foot floor inclination is equal to the actual n-th foot floor inclination and 
20 that the n-th foot of the robot 1 is in contact with the floor completely, the compliance mechanism 42 of the robot 1 
must deform so as to cancel the estimated n-th foot interference angle enint. This mechanism deformation must yield 
the moment of the 1 st foot floor reaction force about the desired 1 st foot floor reaction force central point. This moment 
is called a "M1 estrn". Similarly, this mechanism deformation must also yield the moment of the 2nd foot floor reaction 
force about the desired 2nd foot floor reaction force central point. This moment is called a "M2estm". 
25 [0326] Then , relationship between the estimated n-th foot interference angle's X-component Gnintx and the moment 
component's X-component Mnestmx should be determined beforehand and stored in memory. Among of characteristics 
of the compliance mechanism 42 of the robot 1 , this relationship indicates only a characteristic of the foot interference 
angle and hence is called a "foot mechanism compliance model". This is illustrated in Figure 45 with reference numeral 
134b. 

30 [0327] The configuration of the floor shape estimator 130a according to the second embodiment will then be ex- 
plained. 

[0328] The floor shape estimator 1 30a according to the second embodiment comprises a foot-to-foot floor inclination 
difference estimator 138a and a foot floor inclination difference estimator 144a. Figure 44 is a block diagram showing 
the configuration and processing of the foot-to-foot floor inclination difference estimator 138a. Figure 45 is a block 

35 diagram showing the configuration and processing of the foot floor inclination difference estimator 1 44a. 

[0329] Figure 45 illustrates only the configuration and processing for estimating the 1 st foot floor inclination differ- 
ence's X-component, but those for estimating the 1 st foot floor inclination difference's Y-component, the 2nd foot floor 
inclination difference's X-component and the 2nd foot floor inclination difference's Y-component will be similar. 
[0330] The processing of the foot-to-foot floor inclination difference estimator 138a will be explained with reference 

40 to Figure 44. 

[0331 ] First, the V-direction component 0 errv of the body inclination error 0 errv is calculated by dissolving the error 
in the V-direction component and the component perpendicular thereto. 

[0332] Next, the inclination of the vector whose initial point is the desired 1st foot floor reaction force central point 
Q1 and whose terminal point is the desired 2nd foot floor reaction force central point Q2 of the desired gait, and a 
45 difference obtained by subtracting the supposed foot-to-foot floor inclination from the calculated inclination is deter- 
mined as a "supposed feet interference angle 6 dbintcmdv". 

[0333] Then, the aforesaid estimated feet interference angle Gdbintv is calculated in accordance with the following 

equation: 

50 

edbintv = edbv'+ eerrv - efdbestmv + 6 dbintcmdv Eq. 25 

[0334] Here, as mentioned in thefirst embodiment, efdbestmv is the estimated foot-to-foot floor inclination difference. 
And, edbv' is the feet compensating angle with floor shape estimation and as illustrated in Figure 42, is the sum of the 
55 feet compensating angle edbv and the estimated foot-to-foot floor inclination difference efdbestmv. Since current values 
of the feet compensating angle with floor shape estimation edbv' and the estimated foot-to-foot floor inclination differ- 
ence efdbestmv should be calculated from themselves, they should be stored and their most recent values should be 
used in the calculation. 
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[0335] It should be noted here that, when it is required to estimate the foot-to-foot floor inclination difference before 
a supposed timing at which the feet are to be landed, in order to improve the estimation accuracy, it is preferable to 
add the supposed feet interference angle edbintcmdv. However, the configuration to add the supposed feet compen- 
sating angle edbintcmdv as well as the calculation of this supposed feet interference angle edbintcmdv (in other words, 

5 a portion enclosed by a dash line in Figure 44) is not always indispensable for this system. 

[0336] Then, the aforesaid value Mdbestmv (which is expected to be produced in response to the estimated feet 
interference angle e dbintv) is calculated using the foot-to-foot mechanism compliance model 1 34a, whilst the V-direc- 
tion component Mf1f2actv of the sum of the moments produced about the desired total floor reaction force central point 
by the actual foot floor reaction force's force component of the respective feet is calculated. The V-direction component 

10 is the component obtained by Eqs. 12 and 13 of the composite control. 

[0337] Next, the feet moment estimated difference Mdberr is obtained by calculating a difference between the values 
Mf1f2actv and Mdbestmv. If the estimated foot-to-foot floor inclination difference edbestmv is equal to the actual foot- 
to-foot floor inclination difference, the feet moment estimated difference Mdberr is 0. Namely, if the feet moment esti- 
mated difference Mdberr is 0, the estimated foot-to-foot floor inclination difference Bfdbestmv becomes equal to the 

15 actual foot-to-foot floor inclination difference. 

[0338] Then, in the estimator, the fooMo-foot floor inclination difference estimation interrupter 140a determines 
whether the foot-to-foot floor inclination difference estimation should be conducted or interrupted (discontinued). 
[0339] The foot-to-foot floor inclination difference estimation interrupter 1 40a immediately outputs the aforesaid value 
Mdberr inputted thereto when it determines that the foot-to-foot floor inclination difference estimation should be con- 

20 ducted (continued), whereas it outputs 0 when it determines that the estimation should be interrupted. The details of 
the foot-to-foot floor inclination estimation interrupter 140a will be explained later. 

[0340] Next, the estimator integrates the output from the foot-to-foot floor inclination difference estimation interrupter 
1 40a at an integrator 1 42a by an integration constant Kdbestm'. to determine the estimated foot-to-foot floor inclination 
difference 6 fdbestmv. 

25 [0341] The above is processed by the foot-to-foot floor inclination difference estimator 1 38a according to the second 
embodiment. 

[0342] As illustrated in Figure 42, the estimated foot-to-foot floor inclination difference efdbestmv is added to the feet 
compensating angle 6 dbv, and the sum is inputted to the corrected desired foot position and/or posture calculator 
11 4g as the feet compensating angle with floor shape estimation 6dbv\ 
30 [0343] By repeating the processing mentioned above, the estimated foot-to-foot floor inclination difference efdbestmv 
changes such that the foot-to-foot moment estimated difference Mdberr converges to 0. 

[0344] As mentioned above, when the feet moment estimated difference Mdberr becomes 0, the estimated foot-to- 
foot floor inclination difference e fdbestmv becomes equal to the actual foot-to-foot floor Inclination difference. In this 
way, the estimated foot-to-foot floor inclination difference efdbestmv will converge the actual foot-to-foot floor inclination 
35 difference after a while. Thus, the fooMo-foot floor inclination difference can be estimated in the processing mentioned 
above. 

[0345] it should be noted that when the foot-to-foot mechanism compliance model 134 is linear, the closed-loop 
transfer function from the actual foot-to-foot floor inclination difference estimation to the estimated foot-to-foot floor 
inclination difference estimation in its processing system has a first-order lag. Its time constant T can be obtained by 
*o the following equation, when defining a ratio (in absolute value) of the input (estimated feet interference angle) to the 
foot-to-foot mechanism compliance model 134a to its output (aforesaid moment Mdbestmv): 



T = 1/ (C * Kdbestm') Eq. 26 

45 

[0346] Different form the integration gain Kdbestm in the first embodiment, in the second embodiment, by determining 
it as Kdbestm' and by calculating by the equation mentioned below, it becomes possible to obtain the foot-to-foot floor 
inclination difference estimator 138a having almost the same characteristics as that of the first embodiment. 

50 

Kdbestm' - Kdbestm/C Eq. 27 

[0347] Next, the processing of the foot floor inclination difference estimator 144a will be explained with reference to 
Figure 45. 

55 [0348] Although Figure 45 illustrates only the configuration and processing for estimating the 1 st foot floor inclination 
difference s X-component, they will be also be applied to those for estimating the 1 st foot floor inclination difference's 
Y-component, the 2nd foot floor inclination difference's X-component and the 2nd foot floor inclination difference's Y- 
component. 
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[0349] First, the estimator calculates the 1st foot inclination angle of the desired gait, subtracts the supposed 1st 
foot floor inclination difference therefrom, and determines the X-component of the difference as a supposed 1st foot 
interference angle's X-component 61 intcmdx. 

[0350] Then, the estimator calculates an X-component 61 intx of the aforesaid estimated 1st foot interference angle 
5 using the following equation: 

6 1 intx =6 1x' + 6errx - 6f1 estmx + 6 1 intcmdx Eq. 28 

10 [0351] Here, Oflestmx indicates the estimated 1st foot floor inclination difference's X.-component, as mentioned in 
the first embodiment. And, 6 1 x' indicates the 1 st foot X-compensating angle with floor shape estimation and as shown 
in Figure 42, is the sum of the 1 st foot X-compensating angle 61x and the estimated 1 st foot floor inclination difference's 
X-component 6f1estmx. 

[0352] Since the 1st foot X-compensating angle 61 x' and the estimated 1st foot floor inclination difference's X-com- 
15 ponent Bflestmx are values whose current values should be determined using themselves, their most recent values 
stored in memory should be used in this calculation. 

[0353] It should be noted here that, when it is required to estimate the 1st foot floor inclination difference before a 
supposed timing at which the feet are to be in contact with the floor completely, in order to improve the estimation 
accuracy, it is preferable to add the supposed 1st foot interference angle's X-component 61 intcmdx. However, the 
20 configuration to add the supposed 1st foot compensating angle's X-component 6 1 intcmdx as well as the calculation 
of this supposed 1 st foot interference angle's X-component 9 1 intcmdx (i.e., a portion enclosed by a dash line in Figure 
45) is not always indispensable for this system. 

[0354] Then, the aforesaid value's X-component Mlestmx (which is expected to be produced in response to the 
estimated 1 st foot interference angle's X-component 61 intx) is calculated using the foot mechanism compliance model 
25 134b. 

[0355] Next, the 1st foot floor reaction force moment's estimated difference's X-component Mlerrx is obtained by 
calculating a difference between the detected actual 1st foot floor reaction force's moment M1 actx and the aforesaid 
value Mlestmx. If the estimated 1st foot floor inclination difference 6f1estmx is equal to the actual 1st foot floor incli- 
nation difference, the 1st foot floor reaction force moment's estimated difference's X-component Mlerrx is 0. Saying 
30 this reverse, if the 1st foot floor reaction force moment's estimated difference's X-component Mlerrx is 0, the estimated 
1st foot floor inclination difference's X-component 6 flestmx becomes equal to the actual 1st foot floor inclination 
difference. 

[0356] Then, in the estimator the 1st foot floor inclination difference estimation interrupter determines whether the 
1st foot floor inclination difference estimation should be conducted or interrupted (discontinued). 
35 [0357] The 1st foot floor inclination difference estimation interrupter immediately outputs the 1st foot floor reaction 
force moment's estimated difference's X-component Mlerrx inputted thereto when it determines that the foot-to-foot 
floor inclination difference estimation should be conducted (continued), whereas it outputs 0 when it determines that 
the estimation should be interrupted. The details of the 1st foot floor inclination estimation interrupter 146a will be 
explained later. 

40 [0358] Next, the estimator integrates the output from the 1st foot floor inclination difference estimation interrupter at 
an integrator by an integration constant Klestmx' to determine the estimated 1st foot floor inclination difference's X- 
component 6f 1 estmx. 

[0359] The above is processed by the 1 st foot floor inclination difference estimator according to the second embod- 
iment. 

45 [0360] As illustrated in Figure 42, the estimated 1 st foot floor inclination difference's X-component Of 1 estmx is added 
to the 1 st foot compensating angle 61 x, and the sum is inputted to the corrected desired foot position and/or posture 
calculator 11 4g as the 1st foot's X-compensating angle with floor shape estimation 61 x\ 

[0361] By repeating the processing mentioned above, the estimated 1st foot floor inclination difference's X-compo- 
nent 6f 1 estmx ch anges such that the 1 st foot floor reaction force moment's estimated difference's X-component M 1 errx 
50 converges to 0. 

[0362] As mentioned above, when the 1st foot floor reaction force moment's estimated difference's X-component 
Mlerrx becomes 0, the estimated 1st foot floor inclination difference's X-component 6f1 estmx becomes equal to the 
actual 1 st foot floor inclination difference's X-component. In this way, the estimated 1 st foot floor inclination difference's 
X-component Of 1 estmx will converge the actual 1 st foot floor inclination difference's X-component after a while. Thus, 
55 the 1st foot floor inclination difference's X-component can be estimated in the processing mentioned above. 

[0363] It should be noted that when the foot mechanism compliance model 134b is linear, the closed-loop transfer 
function from the actual 1st foot floor inclination difference's X-component estimation to the estimated 1st foot floor 
inclination difference's X-component estimation in its processing system has a first-order lag. Its time constant T can 
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be obtained by the following equation, when defining a ratio (in absolute value) of the input (estimated 1st foot inter- 
ference angle's X-component e 1 intx) to the foot mechanism compliance model 134b to its output (aforesaid moment 
M1 estmx). 

T= 1/(C1 * Klestmx') Eq. 29 

[0364] Different form the integration gain K1 estmx in the first embodiment, in the second embodiment, by determining 
it as Klestmx' and by calculating by the equation mentioned below, it becomes possible to obtain the 1st foot floor 
io inclination difference estimator having almost the same characteristics as that of the first embodiment. 

K1 estmx* - K1 estmx/C1 Eq. 30 

75 [0365] When the orientation (posture) of the desired foot is different from that of the desired body, it suffices if the 
body inclination is expressed by the each foot's coordinate system. 
[0366] Then, the first embodiment is compared with the second embodiment. 

[0367] In the first embodiment, the finally-corrected desired foot position and/or posture is used as the input to the 
floor shape estimator 130, whilst in the second embodiment, it can be stated that a variable or parameter which is 

20 intermediately generated for determining the finally-corrected desired foot position and/or posture. 

[0368] Summarizing the second embodiment, the compliance model of the first embodiment is approximated by the 
foot-to-foot mechanism compliance model 134a for the feet compensating angle and the foot mechanism compliance 
model 134b for the foot compensating angle, and the block diagram illustrating the configuration is rewritten to be 
equivalent to that of the first embodiment. 

25 [0369] Since the mechanism compliance model is thus simplified in the second embodiment, the estimation accuracy 
is somewhat lower than that of the first embodiment. Since, however, the calculation of the mechanism compliance 
model is simplified, the system according to the second embodiment can decrease the load of the control unit 26. 
Exceptforthis, the rest of the second embodiment is not different from the first embodiment. The effects and advantages 
of the second embodiment are the same as those of the first embodiment. Like the first embodiment, the second 

30 embodiment is sufficient if it can estimate the floor shape estimation. 

[0370] It should be noted in the above, when it is not required to begin estimation of the foot-to-foot floor shape 
difference before a supposed time indicating the start of the two-leg supporting period (i .e. } a supposed time at which 
the free leg lands), the following equation can be used in which the term on the supposed feet interference angle 0 
dbinLcmdv is removed from Eq. 25. This is because the supposed feet interference angle edbintcmdv is 0 during the 

35 two-leg supporting period. 

G dbintv = e dbv'+ 9 errv - 9 fdbestmv Eq. 31 

40 [0371] When it is not required to begin estimation of the 1st foot floor shape difference's X-component before the 
1 st foot's edge along the Y-axis (i.e., the heel edge or toe edge) in the desired gait is to be in contact with the floor, the 
following equation can be used in which the term on the supposed 1 st foot interference angle's X-component 01 intcmdx 
is removed from Eq. 28. The reason is that, a period of time during which the 1st foot's edge along the Y-axis is in 
contact with the floor, the supposed 1st foot interference angle 's X-component 01 intcmdx is 0. 

45 

01intx = 01x" + 9errx - 9f1estmx Eq. 32 

[0372] When it is not required to begin estimation of the 1st foot floor shape difference's Y-component before the 1st 
50 foot's edge along the X-axis (i.e., the inner or outer edge) in the desired gait is to be in contact with the floor, the 
following equation can be used in which the term on the supposed 1st foot interference angle's X-component 9 1 intcmdy 
is removed from an equation (in which x in Eq. 28 is to be replaced by y). The reason is that, a period of time during 
which the 1st foot's edge along the X-axis (i.e. the inner or outer edge) is in contact with the floor, the supposed 1st 
foot interference angle 's Y-component 9 1 intcmdy is 0. 

55 

91inty = G1y' + 0erry - 9f1estmy Eq. 33 
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[0373] Further, if the moment Mf1f2actv obtained from Eq. 12a is used, in stead of the moment Mf1f2actv obtained 
from Eq. 12, the total floor reaction force's moment about the desired total floor reaction force central point (which is 
expect to be produced in response to the mechanism deformation for canceling the estimated feet interference angle) 
should be used, in lieu of the moment Mdeestmv. 
5 [0374] It should be noted that the estimated floor shape difference is stored as information onto floor shape such 
that it can be used as a reference when the robot next walks on the same floor. 

[0375] Then, the processing or operation of the 1st foot floor inclination difference's Y-component estimation inter- 
rupter will be explained. 

[0376] The configuration and processing of the 1st foot floor inclination difference's Y-component estimation inter- 
ne rupter according to the second embodiment is almost the same as that of thefirst embodiment. Specifically, it is sufficient 
in the configuration illustrated in Figure 37, if 0f1errdiffy is replaced by the 1st foot floor reaction force moment's esti- 
mated difference's Y-component M1 erry. 

[0377] Also, the processing of the foot-to-foot floor inclination difference estimation interrupter 140a is almost the 
same as that of thefirst embodiment. Specifically, it is sufficient in the configuration illustrated in Figure 40, if 6 fdberrdiffv 

15 is replaced by the feet moment's estimated difference Mdberr. 

[0378] Explaining the operation of the foot-to-foot floor inclination difference estimation interrupter 1 40a, like the 1 st 
foot floor inclination difference estimation interrupter, it determines whether the estimation of the foot-to-foot floor in- 
clination difference can be conducted normally and if not, it interrupts or discontinues the estimation. More specifically, 
the interrupter outputs 0 in a situation where the estimation accuracy of the foot-to-foot floor inclination difference is 

20 liable to be degraded or where the estimated foot-to-foot floor inclination difference is likely to oscillate, whereas it 
outputs or passes the input immediately to continue the estimation in a situation other than the above. 
[0379] The estimated foot-to-floor inclination difference does not oscillate when the contact pressure between the 
foot and the floor is sufficient, but does oscillate in a situation where one foot is about to be out of the contact with the 
floor, but feet compensating angle is being changed to a direction in which the foot is lifted. Since this is almost the 

25 same as that in the oscillation of the estimated 1 st foot floor inclination difference's Y-component, no further explanation 
will be made. 

[0380] Having been configured in the foregoing manner, the system according to the second embodiment can ac- 
curately estimate the floor shape, more specifically, it can estimate the inclination of the surface with which each foot 
is in contact and a height difference between surfaces with which the feet are in contact, at the same time or simulta- 
30 neously. 

[0381] Further, the system can produce the floor reaction force as desired even when the floor shape is different 
from that has been supposed, and can solve the steady-state error of the floor reaction force due to a deviation of the 
floor shape. 

[0382] Further, since the floor shape estimation is interrupted or discontinued in a situation where the accuracy of 
35 floor shape estimation is liable to degrade or the estimation value is liable to oscillate, in addition to the advantages 
and effects mentioned above, it can prevent the estimated value from being inappropriate. 

[0383] It should be noted that, in the first and second embodiment, a situation where the foot 22R(L) is liable to lift 
is determined by calculating the actual foot floor reaction force central point Qnact or Qnact' from the actual foot floor 
reaction force obtained from the force sensor 34, and by discriminating based on the calculation result which direction 

40 the foot is liable to lift (i.e., discriminating which of the heel, the toe, the inner edge and the outer edge is likely to be lifted). 
[0384] However, the determination should not be limited to that. As illustrated in Figure 46, it is alternatively possible 
to install a pressure-distribution sensor 200 between the foot sole elastic member 40 and the foot main body 22a, and 
to determine from an output signal of the sensor which direction the foot is liable to lift. Since the pressure-distribution 
sensor 200 can detect the deformation of the foot more precisely than the force sensor 34, the provision of such a 

45 sensor will improve the floor shape estimation. 

[0385] Further, as illustrated in Figure 47, it is still alteratively possible to install a plurality of contact sensors 202 at 
the foot 22R(L), for example each at the corners, and to determine from output signals of the sensors which direction 
the foot is liable to lift. In this case, the estimation will be interrupted or discontinued depending on the number of sensor 
output signals (indicative of non-contact of the foot with the floor). 

50 [0386] As mentioned above, the first and second embodiments are configured such that it has a system for controlling 
a legged mobile robot having at least a body 24 and a plurality of legs (leg links) 2 each connected to the body through 
a first joint (hip joints 10,12,1 4R(L)) and having a foot 22R(L) connected to its distal end through a second joint (ankle 
joints 1 8, 20R(L)), the system having a compliance mechanism 42 which deforms in response to a floor reaction force 
acting from a floor with which the foot is in contact and joint displacement means (electric motors) which displacing 

55 tne first and second joints (10, 12R(L),..) such that the foot follows a desired position and/or posture, comprising: 
inclination detection means (inclination sensor 36) for detecting an inclination of the body relative to a vertical direction; 
walking environment foot position and/or posture calculating means (ECU 26, composite compliance operation deter- 
minator 114, corrected desired feet position and/or posture, with deformation compensation calculator 11 4h, adder of 
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the body inclination error's V-direction component eerrv in the foot-to-foot floor inclination difference estimator 138, 
adder of the body inclination error's X-direction component eerrx in the 1st foot floor inclination difference estimator 
144) for calculating a position and/or posture of the foot in a walking environment (corrected desired foot position and/ 
or posture with deformation compensation) including the floor with which the foot is in contact, based on an actual and/ 

5 or desired position and/or posture of the foot and the detected inclination; deformed foot position/posture calculating 
means (ECU 26, composite compliance model 134, floor shape estimator 130 (in particular portions up to that for 
calculating G fdbv' in the foot-to-foot floor inclination difference estimator 138, and portions up to that for calculating 
6f 1 errx 1 in the 1 st foot floor inclination difference estimator 1 44)) for calculating the position and/or posture of the foot 
with deformation of the mechanism (estimated foot position and/or posture after deformation) in response to the floor 

10 reaction force acting from the floor in the walking environment using a mechanism compliance model 1 34 which models 
the compliance mechanism 42; and floor shape estimation value calculating means (ECU 26, composite compliance 
operation determinator 114, floorshape estimator 130, foot-to-foot floor inclination difference estimator 138 (in particular 
portions after that for calculating Gfdberrv), and the foot floor inclination difference estimator 144 (in particular portions 
after that for calculating Gflerrx)) for calculating a floor shape estimation value indicative of shape of surface of the 

75 floor with which each foot is in contact (estimated n-th foot floor inclination (difference) 6 fnestm) and indicative of a 
relationships therebetween (estimated foot-to-foot floor inclination (difference) or estimated foot-to-foot floor height 
difference (error) G fdbestmv). 

[0387] In the system, the compliance model comprises a first model (foot mechanism compliance model 1 34b) which 
expresses the floor reaction force acting on each foot 22R(L) and a change of position and/or posture of each foot 
20 resulting from the floor reaction force acting on each foot, and a second model (foot-to-foot mechanism compliance 
model 1 34a) which expresses the floor reaction force acting at a desired total floor reaction force central point and a 
change of one of the foot relative to the other resulting from the floor reaction force acting at the desired total floor 
reaction force central point. 

[0388] In the system, the floor shape estimation value calculating means has a filter which removes a high-frequency 

25 component (portions for calculating 9 fdbestmv from G fdberrv in the foot-to-foot floor inclination difference estimator 
1 38 and portions for calculating 9f 1 estm from Gf 1 err in the 1 st foot floor inclination difference estimator 1 44). 
[0389] The system further includes: correcting means (ECU 26, adder 132 (132a, 132b), corrected desired foot 
position and/or posture calculator 114g) for correcting the desired position and/or posture of the foot based on the 
calculated floor shape estimation value. 

30 [0390] The system further includes: gait generating means (ECU 26, gait generator 1 00, S 1 0 - S22) for generating 
a gait of the robot 1 including at least a motion pattern of the robot including at least the desired position and/or posture 
of the foot (desired body position and/or posture, desired foot position and/or posture) and a desired pattern of a total 
floor reaction force acting on the robot (desired total floor reaction force, desired total floor reaction force central point 
(desired ZMP)); desired foot floor reaction force central point determining means (ECU 26, desired floor reaction force 

35 distributor 102, S24, S26) for determining a desired foot floor reaction force which is a central point of action on the 
foot (desired foot floor reaction force central point) when the total floor reaction force of the generated gait is distributed 
to each of the foot; actual floor reaction force detecting means (ECU 26, force sensor 34, actual foot floor reaction 
force sensor 1 08. S32) for detecting an actual floor reaction force acting on the foot (actual foot floor reaction force); 
and foot rotation amount determining means (ECU 26, composite compliance operation determinator 114, feet com- 

io pensating angle determinator 114b, n-th foot compensating angle determinator 114c, S32, S34, S100 - S108) for 
calculating a moment of the detected actual floor reaction force acting about the calculated foot floor reaction force 
central point (actual n-th foot floor reaction force's moment Mactx, y, z), and for determining a foot rotation amount for 
rotating the foot (feet compensating angle Gdbv, n-th foot compensating angle G nx, y) based on the calculated moment; 
wherein the correcting means includes: foot rotation amount correcting means (ECU 26, adder 132 (132a, 132b)) for 

■*5 correcting the foot rotation amount based on the calculated floor shape estimation value (feet compensating angle with 
floor shape estimation Gdbv', n-th foot compensating angle with floor shape estimation Gnx', y'); and foot position/ 
posture correcting means (ECU 26, corrected desired foot position/posture calculator 11 4g, S38, S40) for correcting 
the desired position and/or posture of the foot based on the corrected foot rotation amount such that the position/ 
posture of the foot rotates. 

50 [0391] The system further includes: gait generating means (ECU 26, gait generator 100, S10 - S22) for generating 
gait of the robot 1 including at least a motion pattern of the robot including at least the desired position and/or posture 
of the foot 22R(L) (desired body position and/or posture, desired foot position and/or posture) and a desired pattern 
of a total floor reaction force acting on the robot (desired total floor reaction force, desired total floor reaction force's 
central point (desired ZMP)); desired foot floor reaction force central point determining means (ECU 26, desired foot 

55 reaction force distributor 1 02, S24, S26) for determining a desired foot floor reaction force which is a central point of 
action on the foot 22R(L) when the total floor reaction force of the generated gait is distributed to each of the foot 
(desired foot floor reaction force central point); actual floor reaction force detecting means (ECU 26, force sensor 34, 
actual foot floor reaction force sensor 1 08 ; S32) for detecting an actual floor reaction force acting on the foot (actual 
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foot floor reaction force); and foot rotation amount determining means (ECU 26, composite compliance operation de- 
terminator 114, feet compensating angle determinator 114b, n-th foot compensating angle determinator 114c,..., S32, 
S34, S1 00 - S1 08) for determining a foot rotation amount for rotating the foot (feet compensating angle edbv, n-th foot 
compensating angle 9nx,y) based on the detected actual floor reaction force; wherein the correcting, means includes: 

5 foot rotation amount correcting means (ECU 26, adder 1 32 (1 32a, 1 32b)) for correcting the foot rotation amount based 
on the calculated floor shape estimation value (feet compensating angle with floor shape estimation 8dbv\ n-th foot 
compensating angle with floor shape estimation Gnx',y'); and foot position/posture correcting means (ECU 26, corrected 
desired foot position/posture calculator 114g, S38, S40) for correcting the desired position and/or posture of the foot 
based on the corrected foot rotation amounts such that the position and/or posture of the foot rotates about the deter- 

"O mined foot floor reaction force central point (desired foot floor reaction force central point) or thereabout. 

[0392] In the system, the foot position/posture correcting means corrects the desired position and/or posture of the 
foot based on the corrected foot rotation amounts such that the position and/or posture of the foot rotates about the 
determined foot floor reaction force central point or thereabout. 

[0393] The system further includes: foot movement amount determining means (ECU 26, compensating total floor 
15 reaction force's moment distributor 114a, feet compensating angle determinator 114, S100 - S106) for calculating at 
least one of the total floor reaction force's moment actually acting on the robot (more preciseJy a moment component 
PQ1 * F1act + PQ2 * F2act + M1act 4- M2act), and a moment (Mf1f2act =' PQ1 * F1act + PQ2 * F2act) obtained by 
subtracting the moment of the detected actual floor reaction force acting about the calculated foot floor reaction force 
central point (M1 act + M2act), from the total floor reaction force's moment actually acting on the robot (PQ1 * F1act + 
20 PQ2 * F2act + M 1 act + M2act), and for determining a foot movement amount for moving the foot (6dbv) based on the 
calculated moment. 

[0394] In the system, the foot position/posture correcting means further corrects the desired position and/or posture 
of the foot based on an posture error of the robot. 

[0395] In the system, a compensating total floor reaction force's moment for posture stabilization (compensating total 
25 floor reaction force Mdmd) that is to be added to the desired pattern of the total floor reaction force is determined, and 
the foot rotation amount determining means and/or the foot movement amount determining means determines the foot 
rotation amount and/or the foot movement amount based on at least the detected actual floor reaction force (actual 
foot floor reaction force) and the determined compensating total floor reaction force's moment for posture stabilization 
(S34 ; S100 -S108). 

30 [0396] In the system, the compensating total floor reaction force's moment for posture stabilization is determined 
based on an inclination error of the robot (6 errx, y). 

[0397] In the system, the foot rotation amount determining means and/or the foot movement amount determining 
means determines the foot rotation amount and/or the foot movement amount such that the determined compensating 
total floor reaction force's moment for posture stabilization is distributed to each of the legs (S34, S100 - S108). 

35 [0398] The system further includes: gait generating means (ECU 26, gait generator 100, S 10 - S22) for generating 
gait of the robot 1 including at least a motion pattern of the robot including at least the desired position and/or posture 
of the foot 22R(L) (desired body position and/or posture, desired foot position and/or posture) and a desired trajectory 
pattern of a total floor reaction force acting on the robot (desired total floor reaction force, desired total floor reaction 
"orce central point (desired ZMP)); compensating total floor reaction force calculating means (ECU 26, posture stabi- 

40 uzation controller 104, S28, S30) for calculating a compensating total floor reaction force for posture stabilization (com- 
pensating total floor reaction force Mdmd); actual floor reaction force detecting means (force sensor 34, ECU 26 , actual 
taction force sensor 1 08, S32) for detecting an actual floor reaction force acting on the foot (actual foot floor reaction 
force); and floor reaction force distributing means (ECU 26, desired floor reaction force distributor 102, S34, S100 - 
S104) for distributing the total floor reaction force of the generated gait and the compensating total floor reaction for; 

45 wherein the correcting means corrects the desired foot position and/or posture based on the distributed total floor 
reaction force of the generated gait and the compensating total floor reaction force, the detected actual floor reaction 
force and the calculated floor shape estimation value (ECU 26, compensating angle determinators 114b, 114c, adder 
132 (132a, 132b), corrected desired foot position/posture calculator 114g, corrected desired foot position/posture with 
deformation compensation calculator 11 4h, deformation amount calculator 114n). 

50 [0399] In the system, the compensating total floor reaction force for posture stabilization is calculated based on an 
inclination error of the robot (6errx,y). 

[0400] In the first embodiment and the second embodiment, equivalent altering the order calculation may be made 
on the block diagrams illustrated. 

[0401] In the first embodiment and the second embodiment, although the present invention has been described with 
55 reference to a biped robot, the present invention should not be limited to that and can be applied to other legged mobile 
robots. 
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INDUSTRIAL FIELD IN WHICH THE INVENTION IS APPLICABLE 

[0402] According to the present invention, it becomes possible to provide a floor shape estimation system of a legged 
mobile robot which can accurately estimate the inclination of a surface with which the robot foot is in contact and a 
height difference between surfaces with which the robot feet are in contact, at the same time, i.e., in a composite 
manner. Such a simultaneous estimation has been difficult in the prior art. In addition, the system is simple in config- 
uration. 

[0403] Further, in addition to the effects, it becomes possible to provide a floor shape estimation system for a legged 
mobile robot which can bring the steady-state error of the actual floor reaction force from a desired value in the control 
(which the prior art composite control could not eliminate), close to zero as much as possible. In other words, it can 
solve the steady-state error due to a deviation of the floor reaction force. 



Claims 

1 . A system for controlling a legged mobile robot having at least a body and a plurality of legs each connected to the 
body through a first joint and having a foot connected to its distal end through a second joint, the system having 
a compliance mechanism which deforms in response to a floor reaction force acting from a floor with which the 
foot is in contact and joint displacement means which displacing the first and second joints such that the foot 
follows a desired position and/or posture, comprising: 

a inclination detection means for detecting an inclination of the body relative to a vertical direction; 
b walking environment foot position and/or posture calculating means for calculating a position and/or posture 
of the foot in a walking environment including the floor with which the foot is in contact, based on an actual 
and/or desired position and/or posture of the foot and the detected inclination; 

c. deformed foot position/posture calculating means for calculating the position and/or posture of the foot with 
deformation of the mechanism in response to the floor reaction force acting from the floor in the walking en- 
vironment using a mechanism compliance model which models the compliance mechanism; and 

d. floor shape estimation value calculating means for calculating a floor shape estimation value indicative of 
shape of surface of the floor with which each foot is in contact and indicative of a relationships therebetween. 

2. A system according to claim 1 , wherein the compliance model comprises a first model which expresses the floor 
reaction force acting on each foot and a change of position and/or posture of each foot resulting from the floor 
reaction force acting on each foot, and a second model which expresses the floor reaction force acting at a desired 
total floor reaction force central point and a change of one of the foot relative to the other resulting from the floor 
reaction force acting at the desired total floor reaction force central point. 

3. A system according to claim 1 or 2 , wherein the floor shape estimation value calculating means has a filter which 
removes a high-frequency component. 

4. A system according to any of claims 1 to 3, further including: 

e. correcting means for correcting the desired position and/or posture of the foot based on the calculated floor 
shape estimation value. 

5. A system according to claim 4, further including: 

f . gait generating means for generating a gait of the robot including at least a motion pattern of the robot 
including at least the desired position and/or posture of the foot and a desired pattern of a total floor reaction 
force acting on the robot; 

g. desired foot floor reaction force central point determining means for determining a desired foot floor reaction 
force which is a central point of action on the foot when the total floor reaction force of the generated gait is 
distributed to each of the foot; 

h. actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot; and 

i. foot rotation amount determining means for calculating a moment of the detected actual floor reaction force 
acting about the calculated foot floor reaction force central point, and for determining a foot rotation amount 
for rotating the foot based on the calculated moment; 

wherein the correcting means includes: 
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j. foot rotation amount correcting means for correcting the foot rotation amount based on the calculated floor 
shape estimation value; and 

k. foot position/posture correcting means for correcting the desired position and/or posture of the foot based 
on the corrected foot rotation amount such that the position/posture of the foot rotates. 

5 

6. A system according to claim 4, further including: 

I . gait generating means for generating gait of the robot including at least a motion pattern of the robot including 
at least the desired position and/or posture of the foot and a desired pattern of a total floor reaction force acting 
r o on the robot; 

m. desired foot floor reaction force central point determining means for determining a desired foot floor reaction 
force which is a central point of action on the foot when the total floor reaction force of the generated gait is 
distributed to each of the foot; 

n. actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot; and 
■ s o. foot rotation amount determining means for determining a foot rotation amount for rotating the foot based 

on the detected actual floor reaction force; 

wherein the correcting means includes: 
p. foot rotation amount correcting means for correcting the foot rotation amount based on the calculated floor 
shape estimation value; and 

20 q. foot position/posture correcting means for correcting the desired position and/or posture of the foot based 

on the corrected foot rotation amounts such that the position and/or posture of the foot rotates about the 
determined foot floor reaction force central point or thereabout. 

7. A system according to claim 5, wherein the foot position/posture correcting means corrects the desired position 
25 and/or posture of the foot based on the corrected foot rotation amounts such that the position and/or. posture of 

the foot rotates about the determined foot floor reaction force central point or thereabout. 

8. A system according to any of claims 5 to 7, further including: 

so r. foot movement amount determining means for calculating at least one of the total floor reaction force's 

moment actually acting on the robot, and a moment obtained by subtracting the moment of the detected actual 
floor reaction force acting about the calcu lated foot floor reaction force central point, from the total floor reaction 
force's moment actually acting on the robot, and for determining a foot movement amount for moving the foot 
based on the calculated moment. 

55 

9. A system according to any of claims 5 to 8, wherein the foot position/posture correcting means further corrects 
the desired position and/or posture of the foot based on an posture error of the robot. 

10. A system according to any of claims 5 to 9, wherein a compensating total floor reaction force's moment for posture 
40 stabilization that is to be added to the desired pattern of the total floor reaction force is determined, and the foot 

rotation amount determining means and/or the foot movement amount determining means determines the foot 
rotation amount and/or the foot movement amount based on at least the detected actual floor reaction force and 
the determined compensating total floor reaction force's moment for posture stabilization. 

45 11 . A system according to claim 1 0, wherein the compensating total floor reaction force's moment for posture stabili- 
zation is determined based on an inclination error of the robot. 

12. A system according to claim 10 or 11 , wherein the foot rotation amount determining means and/or the foot move- 
ment amount determining means determines the foot rotation amount and/or the foot movement amount such that 

50 the determined compensating total floor reaction force's moment for posture stabilization is distributed to each of 

the legs. 

13. A system according to claim 4, further including: 

55 s. gait generating means for generating gait of the robot including at least a motion pattern of the robot including 

at least the desired position and/or posture of the foot and a desired trajectory pattern of a total floor reaction 
force acting on the robot; 

t. compensating total floor reaction force calculating means for calculating a compensating total floor reaction 
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force for posture stabilization; 

u. actual floor reaction force detecting means for detecting an actual floor reaction force acting on the foot; and 
v. floor reaction force distributing means for distributing the total floor reaction force of the generated gait and 
the compensating total floor reaction force; 

wherein the correcting means corrects the desired foot position and/or posture based on the distributed total 
floor reaction force of the generated gait and the compensating total floor reaction force, the detected actual floor 
reaction force and the calculated floor shape estimation value. 

14. A system according to claim 13, wherein the compensating total floor reaction force for posture stabilization is 
calculated based on an inclination error of the robot. 
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FIG. 8 
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FIG. 19 
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FIG. 20 




TIME 



TIME 



Wlx 



TIME 



TIME 



TIME 



TIME 



TWO-LEG SUPPORTING 
PERIOD 



TIME 



ONE-LEG 

SUPPORTING PERIOD 



* 



2ND FOOT HEEL LIFTING 
1ST FOOT HEEL LANDING 



2ND FOOT HEEL LANDING 
1ST FOOT HEEL LIFTING 



2ND FOOT TOE LIFTING 
1ST FOOT TOE LANDING 



56 



3NSDOCID: <E P 1291136A1.. I. > 



EP 1 291 136 A1 



FIG. 21 
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FIG. 26 
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FIG. 28 
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FIG. 29 
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